diff --git a/.gitignore b/.gitignore new file mode 100644 index 00000000..8f1419f5 --- /dev/null +++ b/.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/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 394d9312..1f1525ca 100644 --- a/CMakeLists.txt +++ b/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() diff --git a/README.md b/README.md index 12d2a236..3f2fff33 100644 --- a/README.md +++ b/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 diff --git a/config/rplidar.yaml b/config/rplidar.yaml new file mode 100644 index 00000000..607d167a --- /dev/null +++ b/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 diff --git a/config/rplidar_a3.yaml b/config/rplidar_a3.yaml new file mode 100644 index 00000000..93e6166e --- /dev/null +++ b/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 diff --git a/config/rplidar_tcp.yaml b/config/rplidar_tcp.yaml new file mode 100644 index 00000000..8976008e --- /dev/null +++ b/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 diff --git a/include/RPLidarNode.hpp b/include/RPLidarNode.hpp new file mode 100644 index 00000000..da9b9902 --- /dev/null +++ b/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 +#pragma GCC diagnostic pop + +#include +#include +#include +#include +#include +#include +#include +#include + +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::SharedPtr; +using StartMotorService = rclcpp::Service::SharedPtr; +using StopMotorService = rclcpp::Service::SharedPtr; +using RPLidarDriver = rp::standalone::rplidar::RPlidarDriver; +using RPLidarScanMode = rp::standalone::rplidar::RplidarScanMode; +using Clock = rclcpp::Clock::SharedPtr; +using ResponseNodeArray = std::array; +using EmptyRequest = std::shared_ptr; +using EmptyResponse = std::shared_ptr; +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; + + [[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_ diff --git a/include/rplidar_node.hpp b/include/rplidar_node.hpp deleted file mode 100644 index 7816a2b1..00000000 --- a/include/rplidar_node.hpp +++ /dev/null @@ -1,130 +0,0 @@ -/* - * 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_ - -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include - -#ifndef _countof -#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) -#endif - -#define M_PI 3.1415926535897932384626433832795 - -namespace -{ -using LaserScan = sensor_msgs::msg::LaserScan; -using LaserScanPub = rclcpp::Publisher::SharedPtr; -using StartMotorService = rclcpp::Service::SharedPtr; -using StopMotorService = rclcpp::Service::SharedPtr; -using RPlidarDriver = rp::standalone::rplidar::RPlidarDriver; -using RplidarScanMode = rp::standalone::rplidar::RplidarScanMode; -using Clock = rclcpp::Clock::SharedPtr; -using ResponseNodeArray = std::unique_ptr; -using EmptyRequest = std::shared_ptr; -using EmptyResponse = std::shared_ptr; -using Timer = rclcpp::TimerBase::SharedPtr; -using namespace std::chrono_literals; -} - -namespace rplidar_ros -{ - -constexpr double deg_2_rad(double x) -{ - return x * M_PI / 180.0; -} - -static float getAngle(const rplidar_response_measurement_node_hq_t& node) -{ - return node.angle_z_q14 * 90.f / 16384.f; -} - -class RPLIDAR_ROS_PUBLIC rplidar_node : public rclcpp::Node -{ -public: - explicit rplidar_node(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - virtual ~rplidar_node(); - - void publish_scan(const double scan_time, 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: - bool getRPLIDARDeviceInfo() const; - bool checkRPLIDARHealth() const; - bool set_scan_mode(); - void publish_loop(); - - /* parameters */ - std::string channel_type_; - std::string tcp_ip_; - std::string serial_port_; - std::string topic_name_; - int tcp_port_; - int serial_baudrate_; - std::string frame_id_; - bool inverted_; - bool angle_compensate_; - int m_angle_compensate_multiple; - std::string scan_mode_; - /* Publisher */ - LaserScanPub m_publisher; - /* Services */ - StopMotorService m_stop_motor_service; - StartMotorService m_start_motor_service; - /* SDK Pointer */ - RPlidarDriver * m_drv = nullptr; - /* Timer */ - Timer m_timer; - /* Scan Times */ - size_t m_scan_count = 0; - double max_distance = 8.0f; - double angle_min = deg_2_rad(0); - double angle_max = deg_2_rad(359); - const float min_distance = 0.15f; -}; - -} // namespace rplidar_ros - -#endif // RPLIDAR_NODE_HPP_ diff --git a/launch/rplidar.launch.py b/launch/rplidar.launch.py index aaca70f9..c3609db2 100644 --- a/launch/rplidar.launch.py +++ b/launch/rplidar.launch.py @@ -1,21 +1,20 @@ -from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node +from os import path + +from launch import LaunchDescription def generate_launch_description(): return LaunchDescription([ Node( - node_name='rplidar_composition', + node_name='rplidar', package='rplidar_ros', - node_executable='rplidar_composition', + node_executable='rplidar', output='screen', - parameters=[{ - 'serial_port': '/dev/ttyUSB0', - 'serial_baudrate': 115200, # A1 / A2 - # 'serial_baudrate': 256000, # A3 - 'frame_id': 'laser', - 'inverted': False, - 'angle_compensate': True, - }], + parameters=[ + path.join(get_package_share_directory('rplidar_ros'), 'config', + 'rplidar.yaml') + ], ), ]) diff --git a/launch/rplidar_a3.launch.py b/launch/rplidar_a3.launch.py index 37a2d49f..d6c92585 100644 --- a/launch/rplidar_a3.launch.py +++ b/launch/rplidar_a3.launch.py @@ -1,21 +1,20 @@ -from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node +from os import path + +from launch import LaunchDescription def generate_launch_description(): return LaunchDescription([ Node( - node_name='rplidar_composition', + node_name='rplidar', package='rplidar_ros', - node_executable='rplidar_composition', + node_executable='rplidar', output='screen', - parameters=[{ - 'serial_port': '/dev/ttyUSB0', - 'serial_baudrate': 256000, # A3 - 'frame_id': 'laser', - 'inverted': False, - 'angle_compensate': True, - 'scan_mode': 'Sensitivity', - }], + parameters=[ + path.join(get_package_share_directory('rplidar_ros'), 'config', + 'rplidar_a3.yaml') + ], ), ]) diff --git a/launch/rplidar_s1.launch.py b/launch/rplidar_s1.launch.py index 26184d12..c3609db2 100644 --- a/launch/rplidar_s1.launch.py +++ b/launch/rplidar_s1.launch.py @@ -1,20 +1,20 @@ -from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node +from os import path + +from launch import LaunchDescription def generate_launch_description(): return LaunchDescription([ Node( - node_name='rplidar_composition', + node_name='rplidar', package='rplidar_ros', - node_executable='rplidar_composition', + node_executable='rplidar', output='screen', - parameters=[{ - 'serial_port': '/dev/ttyUSB0', - 'serial_baudrate': 256000, - 'frame_id': 'laser', - 'inverted': False, - 'angle_compensate': True, - }], + parameters=[ + path.join(get_package_share_directory('rplidar_ros'), 'config', + 'rplidar.yaml') + ], ), ]) diff --git a/launch/rplidar_s1_tcp.launch.py b/launch/rplidar_s1_tcp.launch.py index c285a5e5..a8a38ef7 100644 --- a/launch/rplidar_s1_tcp.launch.py +++ b/launch/rplidar_s1_tcp.launch.py @@ -1,21 +1,20 @@ -from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node +from os import path + +from launch import LaunchDescription def generate_launch_description(): return LaunchDescription([ Node( - node_name='rplidar_composition', + node_name='rplidar', package='rplidar_ros', - node_executable='rplidar_composition', + node_executable='rplidar', output='screen', - parameters=[{ - 'channel_type': 'tcp', - 'tcp_ip': '192.168.0.7', - 'tcp_port': 20108, - 'frame_id': 'laser', - 'inverted': False, - 'angle_compensate': True, - }] + parameters=[ + path.join(get_package_share_directory('rplidar_ros'), 'config', + 'rplidar_tcp.yaml') + ], ), ]) diff --git a/launch/test_rplidar.launch.py b/launch/test_rplidar.launch.py index 6a90222a..76141373 100644 --- a/launch/test_rplidar.launch.py +++ b/launch/test_rplidar.launch.py @@ -1,26 +1,26 @@ -from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node +from os import path + +from launch import LaunchDescription def generate_launch_description(): return LaunchDescription([ Node( - node_name='rplidar_composition', + node_name='rplidar', package='rplidar_ros', - node_executable='rplidar_composition', + node_executable='rplidar', output='screen', - parameters=[{ - 'serial_port': '/dev/ttyUSB0', - 'serial_baudrate': 115200, - 'frame_id': 'laser', - 'inverted': False, - 'angle_compensate': True, - }], + parameters=[ + path.join(get_package_share_directory('rplidar_ros'), 'config', + 'rplidar.yaml') + ], ), Node( - node_name='rplidarNodeClient', + node_name='rplidar_client', package='rplidar_ros', - node_executable='rplidarNodeClient', + node_executable='rplidar_client', output='screen', ), ]) diff --git a/launch/test_rplidar_a3.launch.py b/launch/test_rplidar_a3.launch.py index 33126113..d150e98d 100644 --- a/launch/test_rplidar_a3.launch.py +++ b/launch/test_rplidar_a3.launch.py @@ -1,27 +1,26 @@ -from launch import LaunchDescription +from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node +from os import path + +from launch import LaunchDescription def generate_launch_description(): return LaunchDescription([ Node( - node_name='rplidar_composition', + node_name='rplidar', package='rplidar_ros', - node_executable='rplidar_composition', + node_executable='rplidar', output='screen', - parameters=[{ - 'serial_port': '/dev/ttyUSB0', - 'serial_baudrate': 115200, - 'frame_id': 'laser', - 'inverted': False, - 'angle_compensate': True, - 'scan_mode': 'Sensitivity', - }], + parameters=[ + path.join(get_package_share_directory('rplidar_ros'), 'config', + 'rplidar_a3.yaml') + ], ), Node( - node_name='rplidarNodeClient', + node_name='rplidar_client', package='rplidar_ros', - node_executable='rplidarNodeClient', + node_executable='rplidar_client', output='screen', ), ]) diff --git a/package.xml b/package.xml index 867f30aa..4549ec82 100644 --- a/package.xml +++ b/package.xml @@ -11,13 +11,11 @@ ament_cmake_ros rclcpp - sensor_msgs std_srvs rclcpp_components rclcpp - sensor_msgs std_srvs rclcpp_components diff --git a/sdk/include/rplidar_cmd.h b/sdk/include/rplidar_cmd.h index cfe07604..dcb0554d 100644 --- a/sdk/include/rplidar_cmd.h +++ b/sdk/include/rplidar_cmd.h @@ -33,6 +33,9 @@ */ #pragma once +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wall" +#pragma GCC diagnostic ignored "-Wpedantic" #include "rplidar_protocol.h" @@ -295,3 +298,4 @@ typedef struct _rplidar_response_device_health_t { #if defined(_WIN32) #pragma pack() #endif +#pragma GCC diagnostic pop diff --git a/sdk/include/rplidar_protocol.h b/sdk/include/rplidar_protocol.h index 2f8231b7..d34d5159 100644 --- a/sdk/include/rplidar_protocol.h +++ b/sdk/include/rplidar_protocol.h @@ -33,6 +33,9 @@ */ #pragma once +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wall" +#pragma GCC diagnostic ignored "-Wpedantic" // RP-Lidar Input Packets @@ -70,3 +73,4 @@ typedef struct _rplidar_ans_header_t { #if defined(_WIN32) #pragma pack() #endif +#pragma GCC diagnostic pop diff --git a/sdk/src/arch/linux/net_serial.cpp b/sdk/src/arch/linux/net_serial.cpp index eb1ca719..c26a6cf1 100644 --- a/sdk/src/arch/linux/net_serial.cpp +++ b/sdk/src/arch/linux/net_serial.cpp @@ -94,7 +94,7 @@ bool raw_serial::bind(const char * portname, uint32_t baudrate, uint32_t flags) return true; } -bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t flags) +bool raw_serial::open(const char * portname, uint32_t baudrate, uint32_t) { if (isOpened()) close(); @@ -278,18 +278,18 @@ int raw_serial::recvdata(unsigned char * data, size_t size) } -void raw_serial::flush( _u32 flags) +void raw_serial::flush( _u32) { tcflush(serial_fd,TCIFLUSH); } -int raw_serial::waitforsent(_u32 timeout, size_t * returned_size) +int raw_serial::waitforsent(_u32, size_t * returned_size) { if (returned_size) *returned_size = required_tx_cnt; return 0; } -int raw_serial::waitforrecv(_u32 timeout, size_t * returned_size) +int raw_serial::waitforrecv(_u32, size_t * returned_size) { if (!isOpened() ) return -1; @@ -364,7 +364,7 @@ int raw_serial::waitfordata(size_t data_count, _u32 timeout, size_t * returned_s } // data avaliable - assert (FD_ISSET(serial_fd, &input_set)); + assert (FD_ISSET(serial_fd, &input_set)) if ( ioctl(serial_fd, FIONREAD, returned_size) == -1) return ANS_DEV_ERR; diff --git a/sdk/src/arch/linux/net_socket.cpp b/sdk/src/arch/linux/net_socket.cpp index 0124a13a..81522a6f 100644 --- a/sdk/src/arch/linux/net_socket.cpp +++ b/sdk/src/arch/linux/net_socket.cpp @@ -6,6 +6,9 @@ * POXIS Implementation */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wextra" +#pragma GCC diagnostic ignored "-Wsign-compare" #include "sdkcommon.h" #include "../../hal/socket.h" @@ -851,6 +854,7 @@ DGramSocket * DGramSocket::CreateSocket(SocketBase::socket_family_t family) } - }} +#pragma GCC diagnostic pop + diff --git a/sdk/src/arch/linux/thread.hpp b/sdk/src/arch/linux/thread.hpp index dc77ce79..07e6f6e8 100644 --- a/sdk/src/arch/linux/thread.hpp +++ b/sdk/src/arch/linux/thread.hpp @@ -126,7 +126,7 @@ Thread::priority_val_t Thread::getPriority() return PRIORITY_NORMAL; } -u_result Thread::join(unsigned long timeout) +u_result Thread::join(unsigned long) { if (!this->_handle) return RESULT_OK; diff --git a/sdk/src/hal/thread.cpp b/sdk/src/hal/thread.cpp index 16ccb905..f3325cf9 100644 --- a/sdk/src/hal/thread.cpp +++ b/sdk/src/hal/thread.cpp @@ -32,6 +32,8 @@ * */ +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wcast-function-type" #include "sdkcommon.h" #include "hal/thread.h" @@ -45,4 +47,4 @@ #error no threading implemention found for this platform. #endif - +#pragma GCC diagnostic push diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp index 1cc6774f..ce543a93 100644 --- a/sdk/src/rplidar_driver.cpp +++ b/sdk/src/rplidar_driver.cpp @@ -118,7 +118,7 @@ bool RPlidarDriverImplCommon::isConnected() } -u_result RPlidarDriverImplCommon::reset(_u32 timeout) +u_result RPlidarDriverImplCommon::reset(_u32) { u_result ans; @@ -860,7 +860,7 @@ void RPlidarDriverImplCommon::_dense_capsuleToNormal(const rplidar_response_ int dist_q2; int angle_q6; int syncBit; - const int dist = static_cast(_cached_previous_dense_capsuledata.cabins[pos].distance); + const int dist = static_cast(_cached_previous_dense_capsuledata.cabins[pos].distance); dist_q2 = dist << 2; angle_q6 = (currentAngle_raw_q16 >> 10); syncBit = (((currentAngle_raw_q16 + angleInc_q16) % (360 << 16)) < angleInc_q16) ? 1 : 0; @@ -984,7 +984,7 @@ static _u32 _crc32cal(_u32 crc, void* input, _u16 len) _u8 index; _u8* pch; pch = (unsigned char*)input; - _u8 leftBytes = 4 - len & 0x3; + _u8 leftBytes = (4 - len) & 0x3; for (i = 0; i(dist_predict1) == 0xFFFFFE00) || (dist_predict1 == 0x1FF)) { dist_q2[1] = 0; } else { dist_predict1 = (dist_predict1 << scalelvl1); @@ -1203,7 +1203,7 @@ void RPlidarDriverImplCommon::_ultraCapsuleToNormal(const rplidar_response_ultra } - if ((dist_predict2 == 0xFFFFFE00) || (dist_predict2 == 0x1FF)) { + if ((static_cast(dist_predict2) == 0xFFFFFE00) || (dist_predict2 == 0x1FF)) { dist_q2[2] = 0; } else { dist_predict2 = (dist_predict2 << scalelvl2); @@ -1441,7 +1441,7 @@ u_result RPlidarDriverImplCommon::getScanModeName(char* modeName, _u16 scanModeI return ans; } -u_result RPlidarDriverImplCommon::getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs) +u_result RPlidarDriverImplCommon::getAllSupportedScanModes(std::vector& outModes, _u32) { u_result ans; bool confProtocolSupported = false; @@ -1538,7 +1538,7 @@ u_result RPlidarDriverImplCommon::getScanModeCount(_u16& modeCount, _u32 timeout } -u_result RPlidarDriverImplCommon::startScan(bool force, bool useTypicalScan, _u32 options, RplidarScanMode* outUsedScanMode) +u_result RPlidarDriverImplCommon::startScan(bool force, bool useTypicalScan, _u32, RplidarScanMode* outUsedScanMode) { u_result ans; @@ -1761,7 +1761,7 @@ u_result RPlidarDriverImplCommon::startScanExpress(bool force, _u16 scanMode, _u return RESULT_OK; } -u_result RPlidarDriverImplCommon::stop(_u32 timeout) +u_result RPlidarDriverImplCommon::stop(_u32) { u_result ans; _disableDataGrabbing(); @@ -2196,7 +2196,7 @@ void RPlidarDriverSerial::disconnect() stop(); } -u_result RPlidarDriverSerial::connect(const char * port_path, _u32 baudrate, _u32 flag) +u_result RPlidarDriverSerial::connect(const char * port_path, _u32 baudrate, _u32) { if (isConnected()) return RESULT_ALREADY_DONE; @@ -2238,7 +2238,7 @@ void RPlidarDriverTCP::disconnect() _chanDev->close(); } -u_result RPlidarDriverTCP::connect(const char * ipStr, _u32 port, _u32 flag) +u_result RPlidarDriverTCP::connect(const char * ipStr, _u32 port, _u32) { if (isConnected()) return RESULT_ALREADY_DONE; diff --git a/src/RPLidarNode.cpp b/src/RPLidarNode.cpp new file mode 100644 index 00000000..14fb835a --- /dev/null +++ b/src/RPLidarNode.cpp @@ -0,0 +1,395 @@ +/* + * Rplidar ROS NODE + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. + * http://www.slamtec.com + * + * 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. + * + */ + +#include + +namespace rplidar_ros { + +RPLidarNode::RPLidarNode(const rclcpp::NodeOptions& options) : rclcpp::Node("rplidar_node", options) +{ + const auto& logger = this->get_logger(); + + /* set parameters */ + m_channel_type = this->declare_parameter("channel_type", "serial"); + m_tcp_ip = this->declare_parameter("tcp_ip", "192.168.0.7"); + m_tcp_port = this->declare_parameter("tcp_port", 20108); + m_serial_port = this->declare_parameter("serial_port", "/dev/ttyUSB0"); + m_serial_baudrate = this->declare_parameter("serial_baudrate", 115200); + m_frame_id = this->declare_parameter("frame_id", std::string("laser_frame")); + m_inverted = this->declare_parameter("inverted", false); + m_angle_compensate = this->declare_parameter("angle_compensate", false); + m_scan_mode = this->declare_parameter("scan_mode", std::string()); + m_scan_topic = this->declare_parameter("topic_name", std::string("scan")); + + RCLCPP_INFO(logger, "RPLIDAR running on ROS 2 package rplidar_ros. SDK Version: '%s'", RPLIDAR_SDK_VERSION); + + /* initialize SDK */ + if (m_channel_type == "tcp") { + m_driver = RPLidarDriverUPtr(RPLidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP)); + } else { + m_driver = RPLidarDriverUPtr(RPLidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT)); + } + + if (m_driver == nullptr) { + /* don't start spinning without a driver object */ + RCLCPP_ERROR(logger, "Failed to construct driver"); + throw std::bad_alloc(); + } + + if (m_channel_type == "tcp") { + // make connection... + if (IS_FAIL(m_driver->connect(m_tcp_ip.c_str(), static_cast(m_tcp_port)))) { + std::stringstream error_string; + error_string << "Cannot bind to the specified TCP host: " << m_tcp_ip << ":" << m_tcp_port; + throw std::runtime_error(error_string.str()); + } + + } else { + // make connection... + if (IS_FAIL(m_driver->connect(m_serial_port.c_str(), (_u32)m_serial_baudrate))) { + std::stringstream error_string; + error_string << "Cannot bind to the specified serial port '" << m_serial_port << "'"; + throw std::runtime_error(error_string.str()); + return; + } + } + + if (!print_device_info()) { + throw std::runtime_error("Failed to get device info."); + } + + if (!is_healthy()) { + throw std::runtime_error("Failed the health check."); + } + + m_driver->startMotor(); + + if (!set_scan_mode()) { + m_driver->stop(); + m_driver->stopMotor(); + throw std::runtime_error("Failed to set the scan mode."); + } + + m_publisher = this->create_publisher(m_scan_topic, rclcpp::SystemDefaultsQoS()); + + m_stop_motor_service = this->create_service( + "stop_motor", std::bind(&RPLidarNode::stop_motor, this, std::placeholders::_1, std::placeholders::_2)); + + m_start_motor_service = this->create_service( + "start_motor", std::bind(&RPLidarNode::start_motor, this, std::placeholders::_1, std::placeholders::_2)); + + m_timer = this->create_wall_timer(1ms, std::bind(&RPLidarNode::publish_loop, this)); +} + +RPLidarNode::~RPLidarNode() +{ + m_driver->stop(); + m_driver->stopMotor(); +} + +void RPLidarNode::publish_scan(const double scan_time, const ResponseNodeArray& nodes, size_t node_count) +{ + static size_t scan_count = 0; + sensor_msgs::msg::LaserScan scan_msg; + + /* NOTE(allenh1): time was passed in as a parameter before */ + scan_msg.header.stamp = this->now(); + scan_msg.header.frame_id = m_frame_id; + scan_count++; + + bool reversed = (m_angle_max > m_angle_min); + if (reversed) { + /* NOTE(allenh1): the other case seems impossible? */ + scan_msg.angle_min = M_PI - m_angle_max; + scan_msg.angle_max = M_PI - m_angle_min; + } else { + scan_msg.angle_min = M_PI - m_angle_min; + scan_msg.angle_max = M_PI - m_angle_max; + } + scan_msg.angle_increment = (scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count - 1); + + scan_msg.scan_time = scan_time; + scan_msg.time_increment = scan_time / (double)(node_count - 1); + scan_msg.range_min = m_min_distance; + scan_msg.range_max = m_max_distance; + + scan_msg.intensities.resize(node_count); + scan_msg.ranges.resize(node_count); + bool reverse_data = (!m_inverted && reversed) || (m_inverted && !reversed); + if (!reverse_data) { + for (size_t i = 0; i < node_count; i++) { + float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000; + if (read_value == 0.0) { + scan_msg.ranges[i] = std::numeric_limits::infinity(); + } else { + scan_msg.ranges[i] = read_value; + } + scan_msg.intensities[i] = (float)(nodes[i].quality >> 2); + } + } else { + for (size_t i = 0; i < node_count; i++) { + float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000; + if (read_value == 0.0) { + scan_msg.ranges[node_count - 1 - i] = std::numeric_limits::infinity(); + } else { + scan_msg.ranges[node_count - 1 - i] = read_value; + } + scan_msg.intensities[node_count - 1 - i] = (float)(nodes[i].quality >> 2); + } + } + + m_publisher->publish(scan_msg); +} + +bool RPLidarNode::print_device_info() const +{ + const auto& logger = this->get_logger(); + + u_result op_result; + rplidar_response_device_info_t devinfo; + + op_result = m_driver->getDeviceInfo(devinfo); + if (IS_FAIL(op_result)) { + if (op_result == RESULT_OPERATION_TIMEOUT) { + RCLCPP_ERROR(logger, "Operation time out!"); + } else { + RCLCPP_ERROR(logger, "Error code: '%x'", op_result); + } + return false; + } + + constexpr size_t SERIAL_NUMBER_SIZE{16}; + // print out the device serial number, firmware and hardware version number.. + std::stringstream serial_no{}; + serial_no << std::hex << std::uppercase; + for (size_t index = 0; index < SERIAL_NUMBER_SIZE; ++index) { + serial_no << static_cast(devinfo.serialnum[index]); + } + + struct firmware_version + { + uint8_t minor : 8; + uint8_t major : 8; + } version{*reinterpret_cast(&devinfo.firmware_version)}; + + RCLCPP_INFO(logger, "RPLIDAR S/N: %s", serial_no.str().c_str()); + RCLCPP_INFO(logger, "Firmware Ver: %d.%02d", version.major, version.minor); + RCLCPP_INFO(logger, "Hardware Rev: %d", static_cast(devinfo.hardware_version)); + return true; +} + +bool RPLidarNode::is_healthy() const +{ + rplidar_response_device_health_t healthinfo; + u_result op_result = m_driver->getHealth(healthinfo); + + const auto& logger = this->get_logger(); + if (IS_OK(op_result)) { + RCLCPP_INFO(logger, "RPLidar health status : '%d'", healthinfo.status); + if (healthinfo.status == RPLIDAR_STATUS_ERROR) { + RCLCPP_ERROR(logger, "RPLidar internal error detected. Please reboot the device to retry"); + return false; + } + return true; + } + RCLCPP_ERROR(logger, "Cannot retrieve rplidar health code: '%x'", op_result); + return false; +} + +void RPLidarNode::stop_motor(const EmptyRequest, EmptyResponse) +{ + if (m_driver == nullptr) { + return; + } + + RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__); + m_driver->stop(); + m_driver->stopMotor(); +} + +void RPLidarNode::start_motor(const EmptyRequest, EmptyResponse) +{ + if (m_driver == nullptr) { + return; + } + + RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__); + m_driver->startMotor(); + m_driver->startScan(0, 1); +} + +bool RPLidarNode::set_scan_mode() +{ + const auto& logger = this->get_logger(); + + u_result options_results{}; + RPLidarScanMode current_scan_mode{}; + constexpr auto force_scan{false}; + constexpr auto typical_scan_mode{true}; + constexpr auto scan_options{0}; // don't change this + + if (m_scan_mode.empty()) { + options_results = m_driver->startScan(force_scan, typical_scan_mode, scan_options, ¤t_scan_mode); + } else { + std::vector supported_scan_modes; + options_results = m_driver->getAllSupportedScanModes(supported_scan_modes); + + if (IS_OK(options_results)) { + auto found_scan_mode = + std::find_if(supported_scan_modes.begin(), supported_scan_modes.end(), [this](auto scan_mode) { + return std::string_view(scan_mode.scan_mode) == m_scan_mode; + }); + + if (found_scan_mode == supported_scan_modes.end()) { + RCLCPP_ERROR(logger, + "scan mode `%s' is not supported by lidar, supported modes ('%zd'):", + m_scan_mode.c_str(), + supported_scan_modes.size()); + + for (const auto& scan_mode : supported_scan_modes) { + RCLCPP_ERROR(logger, + "%s: max_distance: %.1f m, Point number: %.1fK", + scan_mode.scan_mode, + scan_mode.max_distance, + (1000 / scan_mode.us_per_sample)); + } + + options_results = RESULT_OPERATION_FAIL; + return false; + } else { + options_results = + m_driver->startScanExpress(force_scan, found_scan_mode->id, scan_options, ¤t_scan_mode); + } + } + } + + /* verify we set the scan mode */ + if (!IS_OK(options_results)) { + RCLCPP_ERROR(logger, "Cannot start scan: '%08x'", options_results); + return false; + } + + const float samples_per_microsecond = 1 / current_scan_mode.us_per_sample; + const float samples_per_second = 1e6 * samples_per_microsecond; + const float samples_per_rotation = samples_per_second / DEFAULT_FREQUENCY; // Each rotation is 1000 ms + const float samples_per_degree = samples_per_rotation / 360.0f; + + m_angle_compensate_multiple = samples_per_degree; // minimum viable theoretical samples per degrees rotated + m_angle_compensate_multiple = std::max(m_angle_compensate_multiple, 1.0f); // minimum of 1 sample per degree + m_max_distance = current_scan_mode.max_distance; + + RCLCPP_INFO(logger, + "current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %.1f", + current_scan_mode.scan_mode, + current_scan_mode.max_distance, + (1000 / current_scan_mode.us_per_sample), + m_angle_compensate_multiple); + return true; +} + +void RPLidarNode::publish_loop() +{ + const auto& logger = this->get_logger(); + ResponseNodeArray sample_nodes{}; + + const auto start_scan_time = this->now(); + size_t sample_count = MAX_SAMPLE_COUNT; // will get adjusted to real sample size after grabbing scan data + u_result op_result = m_driver->grabScanDataHq(&sample_nodes[0], sample_count); + const double scan_duration = (this->now() - start_scan_time).seconds(); + + if (op_result != RESULT_OK) { + return; + } + + op_result = m_driver->ascendScanData(&sample_nodes[0], sample_count); + + m_angle_min = degreesToRadians(0.0f); + m_angle_max = degreesToRadians(359.0f); + + if (op_result == RESULT_OK) { + constexpr auto is_valid_node = [](const auto& node) -> bool { return node.dist_mm_q2 != 0; }; + + const auto end_node = std::find_if(sample_nodes.rbegin(), sample_nodes.rend(), is_valid_node); + const auto start_node = std::find_if(sample_nodes.begin(), sample_nodes.end(), is_valid_node); + + m_angle_min = degreesToRadians(getAngleInDegrees(*start_node)); + m_angle_max = degreesToRadians(getAngleInDegrees(*end_node)); + + /** + * turn this -> [0, 0, 0, 1, 2, 3, 0, 0] + * into this -> [1, 2, 3, 0, 0, 0, 0, 0] + */ + std::rotate(sample_nodes.begin(), start_node, end_node.base()); + const size_t nodes_count = end_node.base() - start_node + 1; + + if (m_angle_compensate) { + /** + * Output a fixed number of lidar points and map them to the closest angle possible + * RPlidar by default will output a variable number of samples around a fixed sample + * Example: + * Number of nodes -> 535 - 543 (not consistent) + * Number of compensation nodes-> 525 (a nice fixed number of samples) + * + * We want 525 of the 543, that are evenly distributed + * Our delta then will be 543/525 = 1.03 + */ + const size_t angle_compensate_nodes_count = 360 * m_angle_compensate_multiple; + ResponseNodeArray angle_compensate_nodes{}; + + float angle_compensation_position = 0; + float angle_compensation_delta = static_cast(nodes_count) / angle_compensate_nodes_count; + for (size_t index = 0; index < angle_compensate_nodes_count; ++index) { + angle_compensate_nodes[index] = sample_nodes[static_cast(angle_compensation_position)]; + RCLCPP_DEBUG(logger, "%f", getAngleInDegrees(angle_compensate_nodes[index])); + angle_compensation_position += angle_compensation_delta; + } + + publish_scan(scan_duration, angle_compensate_nodes, angle_compensate_nodes_count); + + } else { + publish_scan(scan_duration, sample_nodes, nodes_count); + } + + } else if (op_result == RESULT_OPERATION_FAIL) { + RCLCPP_WARN(logger, "Failed to organize data in ascending format. Publishing invalid dat anyways."); + publish_scan(scan_duration, sample_nodes, sample_count); + } +} + +} // namespace rplidar_ros + +#include "rclcpp_components/register_node_macro.hpp" + +RCLCPP_COMPONENTS_REGISTER_NODE(rplidar_ros::RPLidarNode) diff --git a/src/client.cpp b/src/client.cpp index 1b27f6ec..cb6900c7 100644 --- a/src/client.cpp +++ b/src/client.cpp @@ -2,26 +2,26 @@ * Copyright (c) 2014, RoboPeak * All rights reserved. * - * Redistribution and use in source and binary forms, with or without + * 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, + * 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 + * 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, + * 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. * */ @@ -34,33 +34,36 @@ * */ - #include #include -#define ROS_INFO RCUTILS_LOG_INFO +constexpr auto M_PI{3.1415926535897932384626433832795}; -#define M_PI 3.1415926535897932384626433832795 -#define RAD2DEG(x) ((x)*180./M_PI) +constexpr float radians_to_degrees(float radians) +{ + return radians * 180 / M_PI; +} void scanCallback(const std::shared_ptr scan) { - int count = scan->scan_time / scan->time_increment; - ROS_INFO("I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count); - ROS_INFO("angle_range, %f, %f", RAD2DEG(scan->angle_min), RAD2DEG(scan->angle_max)); + const auto& logger = rclcpp::get_logger("RPlidar Client"); + const auto count = static_cast(scan->scan_time / scan->time_increment); + + RCLCPP_INFO(logger, "I heard a laser scan %s[%d]:", scan->header.frame_id.c_str(), count); + RCLCPP_INFO(logger, "angle_range, %f, %f", radians_to_degrees(scan->angle_min), radians_to_degrees(scan->angle_max)); - for(int i = 0; i < count; i++) { - float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); - ROS_INFO(": [%f, %f]", degree, scan->ranges[i]); - } + for (size_t index = 0; index < count; ++index) { + float degree = radians_to_degrees(scan->angle_min + scan->angle_increment * index); + RCLCPP_INFO(logger, ": [%f, %f]", degree, scan->ranges[index]); + } } -int main(int argc, char **argv) +int main(int argc, char** argv) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("rplidar_node_client"); - rclcpp::QoS qos{1}; - auto sub = node->create_subscription("/scan", qos, scanCallback); - rclcpp::spin(node); - return 0; + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("rplidar_node_client"); + rclcpp::QoS qos{1}; + auto sub = node->create_subscription("/scan", qos, scanCallback); + rclcpp::spin(node); + return 0; } diff --git a/src/node.cpp b/src/node.cpp deleted file mode 100644 index 4171276b..00000000 --- a/src/node.cpp +++ /dev/null @@ -1,413 +0,0 @@ -/* - * RPLIDAR ROS NODE - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - */ -/* - * 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. - * - */ - -/* #include "ros/ros.h" */ -#include -#include -#include -#include -#include -#include - -#include - -#include "rplidar.h" - -#ifndef _countof -#define _countof(_Array) (int)(sizeof(_Array) / sizeof(_Array[0])) -#endif - -#define M_PI 3.1415926535897932384626433832795 -#define DEG2RAD(x) ((x)*M_PI/180.) - -#define ROS_INFO RCUTILS_LOG_INFO -#define ROS_ERROR RCUTILS_LOG_ERROR -#define ROS_DEBUG RCUTILS_LOG_DEBUG - -using namespace rp::standalone::rplidar; - -RPlidarDriver * drv = NULL; - -void publish_scan(std::shared_ptr> pub, - rplidar_response_measurement_node_hq_t *nodes, - size_t node_count, rclcpp::Time start, - double scan_time, bool inverted, - float angle_min, float angle_max, - float max_distance, - std::string frame_id) -{ - static int scan_count = 0; - sensor_msgs::msg::LaserScan scan_msg; - - scan_msg.header.stamp = start; - scan_msg.header.frame_id = frame_id; - scan_count++; - - bool reversed = (angle_max > angle_min); - if ( reversed ) { - scan_msg.angle_min = M_PI - angle_max; - scan_msg.angle_max = M_PI - angle_min; - } else { - scan_msg.angle_min = M_PI - angle_min; - scan_msg.angle_max = M_PI - angle_max; - } - scan_msg.angle_increment = - (scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count-1); - - scan_msg.scan_time = scan_time; - scan_msg.time_increment = scan_time / (double)(node_count-1); - scan_msg.range_min = 0.15; - scan_msg.range_max = max_distance;//8.0; - - scan_msg.intensities.resize(node_count); - scan_msg.ranges.resize(node_count); - bool reverse_data = (!inverted && reversed) || (inverted && !reversed); - if (!reverse_data) { - for (size_t i = 0; i < node_count; i++) { - float read_value = (float) nodes[i].dist_mm_q2/4.0f/1000; - if (read_value == 0.0) - scan_msg.ranges[i] = std::numeric_limits::infinity(); - else - scan_msg.ranges[i] = read_value; - scan_msg.intensities[i] = (float) (nodes[i].quality >> 2); - } - } else { - for (size_t i = 0; i < node_count; i++) { - float read_value = (float)nodes[i].dist_mm_q2/4.0f/1000; - if (read_value == 0.0) - scan_msg.ranges[node_count-1-i] = std::numeric_limits::infinity(); - else - scan_msg.ranges[node_count-1-i] = read_value; - scan_msg.intensities[node_count-1-i] = (float) (nodes[i].quality >> 2); - } - } - - pub->publish(scan_msg); -} - -bool getRPLIDARDeviceInfo(RPlidarDriver * drv) -{ - u_result op_result; - rplidar_response_device_info_t devinfo; - - op_result = drv->getDeviceInfo(devinfo); - if (IS_FAIL(op_result)) { - if (op_result == RESULT_OPERATION_TIMEOUT) { - ROS_ERROR("Error, operation time out. RESULT_OPERATION_TIMEOUT! "); - } else { - ROS_ERROR("Error, unexpected error, code: %x",op_result); - } - return false; - } - - // print out the device serial number, firmware and hardware version number.. - printf("RPLIDAR S/N: "); - for (int pos = 0; pos < 16 ;++pos) { - printf("%02X", devinfo.serialnum[pos]); - } - printf("\n"); - ROS_INFO("Firmware Ver: %d.%02d",devinfo.firmware_version>>8, devinfo.firmware_version & 0xFF); - ROS_INFO("Hardware Rev: %d",(int)devinfo.hardware_version); - return true; -} - -bool checkRPLIDARHealth(RPlidarDriver * drv) -{ - u_result op_result; - rplidar_response_device_health_t healthinfo; - - op_result = drv->getHealth(healthinfo); - if (IS_OK(op_result)) { - ROS_INFO("RPLidar health status : %d", healthinfo.status); - if (healthinfo.status == RPLIDAR_STATUS_ERROR) { - ROS_ERROR("Error, rplidar internal error detected. Please reboot the device to retry."); - return false; - } else { - return true; - } - - } else { - ROS_ERROR("Error, cannot retrieve rplidar health code: %x", op_result); - return false; - } -} - -bool stop_motor(const std::shared_ptr req, - std::shared_ptr res) -{ - if(!drv) - return false; - - ROS_DEBUG("Stop motor"); - drv->stop(); - drv->stopMotor(); - return true; -} - -bool start_motor(const std::shared_ptr req, - std::shared_ptr res) -{ - if(!drv) - return false; - ROS_DEBUG("Start motor"); - drv->startMotor(); - drv->startScan(0,1); - return true; -} - -static float getAngle(const rplidar_response_measurement_node_hq_t& node) -{ - return node.angle_z_q14 * 90.f / 16384.f; -} - -int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("rplidar_node"); - /* connect to ROS clock */ - rclcpp::Clock::SharedPtr clock; - rclcpp::TimeSource timesource; - clock = std::make_shared(RCL_ROS_TIME); - timesource.attachClock(clock); - - std::string channel_type; - std::string tcp_ip; - std::string serial_port; - int tcp_port = 20108; - int serial_baudrate = 115200; - std::string frame_id; - bool inverted = false; - bool angle_compensate = true; - float max_distance = 8.0; - int angle_compensate_multiple = 1;//it stand of angle compensate at per 1 degree - std::string scan_mode; - // ros::NodeHandle nh; - // auto scan_pub = nh.advertise("scan", 1000); - - /* scan publisher */ - auto scan_pub = node->create_publisher("scan", 10); - - /* set parameters */ - node->get_parameter_or("channel_type", channel_type, std::string("serial")); - node->get_parameter_or("tcp_ip", tcp_ip, std::string("192.168.0.7")); - node->get_parameter_or("tcp_port", tcp_port, 20108); - node->get_parameter_or("serial_port", serial_port, std::string("/dev/ttyUSB0")); - node->get_parameter_or("serial_baudrate", serial_baudrate, 115200); - node->get_parameter_or("frame_id", frame_id, std::string("laser_frame")); - node->get_parameter_or("inverted", inverted, false); - node->get_parameter_or("angle_compensate", angle_compensate, false); - node->get_parameter_or("scan_mode", scan_mode, std::string()); - - ROS_INFO("RPLIDAR running on ROS package rplidar_ros. SDK Version:"RPLIDAR_SDK_VERSION""); - - u_result op_result; - - // create the driver instance - if(channel_type == "tcp"){ - drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP); - } - else{ - drv = RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT); - } - - - if (!drv) { - ROS_ERROR("Create Driver fail, exit"); - return -2; - } - - if(channel_type == "tcp"){ - // make connection... - if (IS_FAIL(drv->connect(tcp_ip.c_str(), (_u32)tcp_port))) { - ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str()); - RPlidarDriver::DisposeDriver(drv); - return -1; - } - - } - else{ - // make connection... - if (IS_FAIL(drv->connect(serial_port.c_str(), (_u32)serial_baudrate))) { - ROS_ERROR("Error, cannot bind to the specified serial port %s.",serial_port.c_str()); - RPlidarDriver::DisposeDriver(drv); - return -1; - } - - } - - // get rplidar device info - if (!getRPLIDARDeviceInfo(drv)) { - return -1; - } - - // check health... - if (!checkRPLIDARHealth(drv)) { - RPlidarDriver::DisposeDriver(drv); - return -1; - } - - // create stop motor service - auto stop_motor_service = node->create_service( - "stop_motor", - std::bind(stop_motor, std::placeholders::_1, std::placeholders::_2)); - - // create start motor service - auto start_motor_service = node->create_service( - "start_motor", - std::bind(start_motor, std::placeholders::_1, std::placeholders::_2)); - - drv->startMotor(); - - RplidarScanMode current_scan_mode; - if (scan_mode.empty()) { - op_result = drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, ¤t_scan_mode); - } else { - std::vector allSupportedScanModes; - op_result = drv->getAllSupportedScanModes(allSupportedScanModes); - - if (IS_OK(op_result)) { - _u16 selectedScanMode = _u16(-1); - for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { - if (iter->scan_mode == scan_mode) { - selectedScanMode = iter->id; - break; - } - } - - if (selectedScanMode == _u16(-1)) { - ROS_ERROR("scan mode `%s' is not supported by lidar, supported modes:", scan_mode.c_str()); - for (std::vector::iterator iter = allSupportedScanModes.begin(); iter != allSupportedScanModes.end(); iter++) { - ROS_ERROR("\t%s: max_distance: %.1f m, Point number: %.1fK", iter->scan_mode, - iter->max_distance, (1000/iter->us_per_sample)); - } - op_result = RESULT_OPERATION_FAIL; - } else { - op_result = drv->startScanExpress(false /* not force scan */, selectedScanMode, 0, ¤t_scan_mode); - } - } - } - - if(IS_OK(op_result)) - { - //default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us - angle_compensate_multiple = (int)(1000*1000/current_scan_mode.us_per_sample/10.0/360.0); - if(angle_compensate_multiple < 1) - angle_compensate_multiple = 1; - max_distance = current_scan_mode.max_distance; - ROS_INFO("current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d", current_scan_mode.scan_mode, - current_scan_mode.max_distance, (1000/current_scan_mode.us_per_sample), angle_compensate_multiple); - } - else - { - ROS_ERROR("Can not start scan: %08x!", op_result); - } - - rclcpp::Time start_scan_time; - rclcpp::Time end_scan_time; - double scan_duration; - while (rclcpp::ok()) { - rplidar_response_measurement_node_hq_t nodes[360*8]; - size_t count = _countof(nodes); - - start_scan_time = clock->now(); - op_result = drv->grabScanDataHq(nodes, count); - end_scan_time = clock->now(); - scan_duration = (end_scan_time - start_scan_time).nanoseconds() * 1E-9; - - if (op_result == RESULT_OK) { - op_result = drv->ascendScanData(nodes, count); - float angle_min = DEG2RAD(0.0f); - float angle_max = DEG2RAD(359.0f); - if (op_result == RESULT_OK) { - if (angle_compensate) { - //const int angle_compensate_multiple = 1; - const int angle_compensate_nodes_count = 360*angle_compensate_multiple; - int angle_compensate_offset = 0; - rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count]; - memset(angle_compensate_nodes, 0, angle_compensate_nodes_count*sizeof(rplidar_response_measurement_node_hq_t)); - - int i = 0, j = 0; - for( ; i < count; i++ ) { - if (nodes[i].dist_mm_q2 != 0) { - float angle = getAngle(nodes[i]); - int angle_value = (int)(angle * angle_compensate_multiple); - if ((angle_value - angle_compensate_offset) < 0) angle_compensate_offset = angle_value; - for (j = 0; j < angle_compensate_multiple; j++) { - angle_compensate_nodes[angle_value-angle_compensate_offset+j] = nodes[i]; - } - } - } - - publish_scan(scan_pub, angle_compensate_nodes, angle_compensate_nodes_count, - start_scan_time, scan_duration, inverted, - angle_min, angle_max, max_distance, - frame_id); - } else { - int start_node = 0, end_node = 0; - int i = 0; - // find the first valid node and last valid node - while (nodes[i++].dist_mm_q2 == 0); - start_node = i-1; - i = count -1; - while (nodes[i--].dist_mm_q2 == 0); - end_node = i+1; - - angle_min = DEG2RAD(getAngle(nodes[start_node])); - angle_max = DEG2RAD(getAngle(nodes[end_node])); - - publish_scan(scan_pub, &nodes[start_node], end_node-start_node +1, - start_scan_time, scan_duration, inverted, - angle_min, angle_max, max_distance, - frame_id); - } - } else if (op_result == RESULT_OPERATION_FAIL) { - // All the data is invalid, just publish them - float angle_min = DEG2RAD(0.0f); - float angle_max = DEG2RAD(359.0f); - - publish_scan(scan_pub, nodes, count, - start_scan_time, scan_duration, inverted, - angle_min, angle_max, max_distance, - frame_id); - } - } - - // rclcpp::spin(node); - } - - // done! - drv->stop(); - drv->stopMotor(); - RPlidarDriver::DisposeDriver(drv); - return 0; -} diff --git a/src/standalone_rplidar.cpp b/src/rplidar.cpp similarity index 81% rename from src/standalone_rplidar.cpp rename to src/rplidar.cpp index c75c261e..b2d9a3d3 100644 --- a/src/standalone_rplidar.cpp +++ b/src/rplidar.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include -#include #include int main(int argc, char ** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); + rclcpp::shutdown(); + return 0; } diff --git a/src/rplidar_node.cpp b/src/rplidar_node.cpp deleted file mode 100644 index e447881e..00000000 --- a/src/rplidar_node.cpp +++ /dev/null @@ -1,392 +0,0 @@ -/* - * Rplidar ROS NODE - * - * Copyright (c) 2009 - 2014 RoboPeak Team - * http://www.robopeak.com - * Copyright (c) 2014 - 2016 Shanghai Slamtec Co., Ltd. - * http://www.slamtec.com - * - * 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. - * - */ - -#include - -namespace rplidar_ros -{ - -rplidar_node::rplidar_node(const rclcpp::NodeOptions & options) -: rclcpp::Node("rplidar_node", options) -{ - /* set parameters */ - channel_type_ = this->declare_parameter("channel_type", "serial"); - tcp_ip_ = this->declare_parameter("tcp_ip", "192.168.0.7"); - tcp_port_ = this->declare_parameter("tcp_port", 20108); - serial_port_ = this->declare_parameter("serial_port", "/dev/ttyUSB0"); - serial_baudrate_ = this->declare_parameter("serial_baudrate", 115200); - frame_id_ = this->declare_parameter("frame_id", std::string("laser_frame")); - inverted_ = this->declare_parameter("inverted", false); - angle_compensate_ = this->declare_parameter("angle_compensate", false); - scan_mode_ = this->declare_parameter("scan_mode", std::string()); - topic_name_ = this->declare_parameter("topic_name", std::string("scan")); - - RCLCPP_INFO(this->get_logger(), - "RPLIDAR running on ROS 2 package rplidar_ros. SDK Version: '%s'", RPLIDAR_SDK_VERSION); - - /* initialize SDK */ - m_drv = (channel_type_ == "tcp") ? - RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP) : - RPlidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT); - - if (nullptr == m_drv) { - /* don't start spinning without a driver object */ - RCLCPP_ERROR(this->get_logger(), "Failed to construct driver"); - return; - } - - if (channel_type_ == "tcp") { - // make connection... - if (IS_FAIL(m_drv->connect(tcp_ip_.c_str(), (_u32)tcp_port_))) { - RCLCPP_ERROR(this->get_logger(), - "Error, cannot bind to the specified TCP host '%s:%ud'", - tcp_ip_.c_str(), static_cast(tcp_port_)); - RPlidarDriver::DisposeDriver(m_drv); - return; - } - } else { - // make connection... - if (IS_FAIL(m_drv->connect(serial_port_.c_str(), (_u32)serial_baudrate_))) { - RCLCPP_ERROR( - this->get_logger(), "Error, cannot bind to the specified serial port '%s'.", - serial_port_.c_str()); - RPlidarDriver::DisposeDriver(m_drv); - return; - } - } - - // get rplidar device info - if (!getRPLIDARDeviceInfo()) { - /* don't continue */ - RPlidarDriver::DisposeDriver(m_drv); - return; - } - - // check health... - if (!checkRPLIDARHealth()) { - RPlidarDriver::DisposeDriver(m_drv); - return; - } - - /* start motor */ - m_drv->startMotor(); - - if (!set_scan_mode()) { - /* set the scan mode */ - m_drv->stop(); - m_drv->stopMotor(); - RPlidarDriver::DisposeDriver(m_drv); - exit(1); - } - - /* done setting up RPLIDAR stuff, now set up ROS 2 stuff */ - - /* create the publisher for "/scan" */ - m_publisher = this->create_publisher(topic_name_, 10); - - /* create stop motor service */ - m_stop_motor_service = this->create_service( - "stop_motor", - std::bind(&rplidar_node::stop_motor, this, std::placeholders::_1, std::placeholders::_2)); - - /* create start motor service */ - m_start_motor_service = this->create_service( - "start_motor", - std::bind(&rplidar_node::start_motor, this, std::placeholders::_1, std::placeholders::_2)); - /* start timer */ - m_timer = this->create_wall_timer(1ms, std::bind(&rplidar_node::publish_loop, this)); -} - -rplidar_node::~rplidar_node() -{ - m_drv->stop(); - m_drv->stopMotor(); - RPlidarDriver::DisposeDriver(m_drv); -} - -void rplidar_node::publish_scan( - const double scan_time, ResponseNodeArray nodes, size_t node_count) -{ - static size_t scan_count = 0; - sensor_msgs::msg::LaserScan scan_msg; - - /* NOTE(allenh1): time was passed in as a parameter before */ - scan_msg.header.stamp = this->now(); - scan_msg.header.frame_id = frame_id_; - scan_count++; - - bool reversed = (angle_max > angle_min); - if (reversed) { - /* NOTE(allenh1): the other case seems impossible? */ - scan_msg.angle_min = M_PI - angle_max; - scan_msg.angle_max = M_PI - angle_min; - } else { - scan_msg.angle_min = M_PI - angle_min; - scan_msg.angle_max = M_PI - angle_max; - } - scan_msg.angle_increment = - (scan_msg.angle_max - scan_msg.angle_min) / (double)(node_count - 1); - - scan_msg.scan_time = scan_time; - scan_msg.time_increment = scan_time / (double)(node_count - 1); - scan_msg.range_min = min_distance; - scan_msg.range_max = max_distance; - - scan_msg.intensities.resize(node_count); - scan_msg.ranges.resize(node_count); - bool reverse_data = (!inverted_ && reversed) || (inverted_ && !reversed); - if (!reverse_data) { - for (size_t i = 0; i < node_count; i++) { - float read_value = (float) nodes[i].dist_mm_q2 / 4.0f / 1000; - if (read_value == 0.0) { - scan_msg.ranges[i] = std::numeric_limits::infinity(); - } else { - scan_msg.ranges[i] = read_value; - } - scan_msg.intensities[i] = (float) (nodes[i].quality >> 2); - } - } else { - for (size_t i = 0; i < node_count; i++) { - float read_value = (float)nodes[i].dist_mm_q2 / 4.0f / 1000; - if (read_value == 0.0) { - scan_msg.ranges[node_count - 1 - i] = std::numeric_limits::infinity(); - } else { - scan_msg.ranges[node_count - 1 - i] = read_value; - } - scan_msg.intensities[node_count - 1 - i] = (float) (nodes[i].quality >> 2); - } - } - - m_publisher->publish(scan_msg); -} - - -bool rplidar_node::getRPLIDARDeviceInfo() const -{ - u_result op_result; - rplidar_response_device_info_t devinfo; - - op_result = m_drv->getDeviceInfo(devinfo); - if (IS_FAIL(op_result)) { - if (op_result == RESULT_OPERATION_TIMEOUT) { - RCLCPP_ERROR(this->get_logger(), "Error, operation time out. RESULT_OPERATION_TIMEOUT!"); - } else { - RCLCPP_ERROR(this->get_logger(), "Error, unexpected error, code: '%x'", op_result); - } - return false; - } - - // print out the device serial number, firmware and hardware version number.. - std::string serial_no{"RPLIDAR S/N: "}; - for (int pos = 0; pos < 16; ++pos) { - char buff[3]; - snprintf(buff, sizeof(buff), "%02X", devinfo.serialnum[pos]); - serial_no += buff; - } - RCLCPP_INFO(this->get_logger(), "%s", serial_no.c_str()); - RCLCPP_INFO( - this->get_logger(), "Firmware Ver: %d.%02d", devinfo.firmware_version >> 8, - devinfo.firmware_version & 0xFF); - RCLCPP_INFO(this->get_logger(), "Hardware Rev: %d", static_cast(devinfo.hardware_version)); - return true; -} - -bool rplidar_node::checkRPLIDARHealth() const -{ - rplidar_response_device_health_t healthinfo; - u_result op_result = m_drv->getHealth(healthinfo); - - if (IS_OK(op_result)) { - RCLCPP_INFO(this->get_logger(), "RPLidar health status : '%d'", healthinfo.status); - if (healthinfo.status == RPLIDAR_STATUS_ERROR) { - RCLCPP_ERROR( - this->get_logger(), - "Error, rplidar internal error detected. Please reboot the device to retry"); - return false; - } - return true; - } - RCLCPP_ERROR(this->get_logger(), "Error, cannot retrieve rplidar health code: '%x'", op_result); - return false; -} - -void rplidar_node::stop_motor(const EmptyRequest req, EmptyResponse res) -{ - if (nullptr == m_drv) { - return; - } - - RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__); - m_drv->stop(); - m_drv->stopMotor(); -} - -void rplidar_node::start_motor(const EmptyRequest req, EmptyResponse res) -{ - if (nullptr == m_drv) { - return; - } - - RCLCPP_DEBUG(this->get_logger(), "Call to '%s'", __FUNCTION__); - m_drv->startMotor(); - m_drv->startScan(0, 1); -} - -bool rplidar_node::set_scan_mode() -{ - u_result op_result; - RplidarScanMode current_scan_mode; - if (scan_mode_.empty()) { - op_result = m_drv->startScan(false /* not force scan */, true /* use typical scan mode */, 0, - ¤t_scan_mode); - } else { - std::vector allSupportedScanModes; - op_result = m_drv->getAllSupportedScanModes(allSupportedScanModes); - if (IS_OK(op_result)) { - auto iter = std::find_if(allSupportedScanModes.begin(), allSupportedScanModes.end(), - [this](auto s1) { - return std::string(s1.scan_mode) == scan_mode_; - }); - if (iter == allSupportedScanModes.end()) { - RCLCPP_ERROR( - this->get_logger(), "scan mode `%s' is not supported by lidar, supported modes ('%zd'):", - scan_mode_.c_str(), allSupportedScanModes.size()); - for (const auto & it : allSupportedScanModes) { - RCLCPP_ERROR(this->get_logger(), "%s: max_distance: %.1f m, Point number: %.1fK", - it.scan_mode, it.max_distance, (1000 / it.us_per_sample)); - } - op_result = RESULT_OPERATION_FAIL; - return false; - } else { - op_result = m_drv->startScanExpress(false /* not force scan */, iter->id, 0, - ¤t_scan_mode); - } - } - } - - /* verify we set the scan mode */ - if (!IS_OK(op_result)) { - RCLCPP_ERROR(this->get_logger(), "Cannot start scan: '%08x'", op_result); - return false; - } - - // default frequent is 10 hz (by motor pwm value), current_scan_mode.us_per_sample is the number of scan point per us - m_angle_compensate_multiple = - static_cast(1000 * 1000 / current_scan_mode.us_per_sample / 10.0 / 360.0); - if (m_angle_compensate_multiple < 1) { - m_angle_compensate_multiple = 1; - } - max_distance = current_scan_mode.max_distance; - RCLCPP_INFO( - this->get_logger(), - "current scan mode: %s, max_distance: %.1f m, Point number: %.1fK , angle_compensate: %d", current_scan_mode.scan_mode, - current_scan_mode.max_distance, (1000 / current_scan_mode.us_per_sample), - m_angle_compensate_multiple); - return true; -} - -void rplidar_node::publish_loop() -{ - rclcpp::Time start_scan_time; - rclcpp::Time end_scan_time; - u_result op_result; - size_t count = 360 * 8; - auto nodes = std::make_unique(count); - - start_scan_time = this->now(); - op_result = m_drv->grabScanDataHq(nodes.get(), count); - end_scan_time = this->now(); - double scan_duration = (end_scan_time - start_scan_time).nanoseconds() * 1E-9; - - if (op_result != RESULT_OK) { - return; - } - op_result = m_drv->ascendScanData(nodes.get(), count); - angle_min = deg_2_rad(0.0f); - angle_max = deg_2_rad(359.0f); - if (op_result == RESULT_OK) { - if (angle_compensate_) { - const int angle_compensate_nodes_count = 360 * m_angle_compensate_multiple; - int angle_compensate_offset = 0; - rplidar_response_measurement_node_hq_t angle_compensate_nodes[angle_compensate_nodes_count]; - memset(angle_compensate_nodes, 0, - angle_compensate_nodes_count * sizeof(rplidar_response_measurement_node_hq_t)); - - size_t i = 0, j = 0; - for (; i < count; i++) { - if (nodes[i].dist_mm_q2 != 0) { - float angle = getAngle(nodes[i]); - int angle_value = (int)(angle * m_angle_compensate_multiple); - if ((angle_value - angle_compensate_offset) < 0) {angle_compensate_offset = angle_value;} - for (j = 0; j < m_angle_compensate_multiple; j++) { - angle_compensate_nodes[angle_value - angle_compensate_offset + j] = nodes[i]; - } - } - } - - publish_scan(scan_duration, std::move(nodes), count); - } else { - int start_node = 0, end_node = 0; - int i = 0; - // find the first valid node and last valid node - while (nodes[i++].dist_mm_q2 == 0) {} - start_node = i - 1; - i = count - 1; - while (nodes[i--].dist_mm_q2 == 0) {} - end_node = i + 1; - - angle_min = deg_2_rad(getAngle(nodes[start_node])); - angle_max = deg_2_rad(getAngle(nodes[end_node])); - auto valid = std::make_unique( - end_node - start_node + 1); - for (size_t x = start_node, y = 0; x < end_node; ++x, ++y) { - valid[y] = nodes[x]; - } - publish_scan(scan_duration, std::move(valid), end_node - start_node + 1); - } - } else if (op_result == RESULT_OPERATION_FAIL) { - // All the data is invalid, just publish them - float angle_min = deg_2_rad(0.0f); - float angle_max = deg_2_rad(359.0f); - - publish_scan(scan_duration, std::move(nodes), count); - } -} - -} // namespace rplidar_ros - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(rplidar_ros::rplidar_node) diff --git a/src/standalone_prlidar.cpp b/src/standalone_prlidar.cpp deleted file mode 100644 index c75c261e..00000000 --- a/src/standalone_prlidar.cpp +++ /dev/null @@ -1,25 +0,0 @@ -// Copyright 2019 Hunter L. allen -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared(rclcpp::NodeOptions())); - rclcpp::shutdown(); - return 0; -}