diff --git a/CMakeLists.txt b/CMakeLists.txt index 637a808b..508cce0a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,8 @@ cmake_minimum_required(VERSION 3.9.5) project(rplidar_ros2) +option(RPLIDAR_BUILD_HITL_SUPPORT "Build HITL simulation support." ON) + if (NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif () @@ -23,6 +25,22 @@ add_library(rplidar_sdk SHARED sdk/src/hal/thread.cpp) +list(APPEND sim_dep) + +if (RPLIDAR_BUILD_HITL_SUPPORT) + message(STATUS "Build HITL support") + #target_sources(rplidar_sdk PRIVATE sdk/src/rplidar_driver.cpp) + add_definitions(-DHITL_SUPPORT) + find_package(Protobuf REQUIRED) + find_package(gz-cmake3 REQUIRED) + find_package(gz-sim7 REQUIRED) + find_package(gz-msgs9 REQUIRED) + set(Protobuf_IMPORT_DIRS ${gz-msgs9_INCLUDE_DIRS}) + target_link_libraries(rplidar_sdk + PRIVATE gz-sim7::gz-sim7 + ) +endif () + target_include_directories(rplidar_sdk PUBLIC sdk/include diff --git a/include/RPLidarNode.hpp b/include/RPLidarNode.hpp index e25b53e5..ad1706cc 100644 --- a/include/RPLidarNode.hpp +++ b/include/RPLidarNode.hpp @@ -110,6 +110,7 @@ class RPLIDAR_ROS_PUBLIC RPLidarNode : public rclcpp::Node std::string m_channel_type; std::string m_tcp_ip; std::string m_serial_port; + std::string m_sim_world_model; std::string m_scan_topic; int m_tcp_port; int m_serial_baudrate; diff --git a/launch/sensors_launch.py b/launch/sensors_launch.py index 4a6aa8b7..9ae23956 100755 --- a/launch/sensors_launch.py +++ b/launch/sensors_launch.py @@ -23,6 +23,8 @@ def generate_launch_description(): # Environment variables DRONE_DEVICE_ID = os.getenv('DRONE_DEVICE_ID') + HITL_WORLD_ID = os.getenv('HITL_WORLD_ID') + hitl_world_id_expr = (HITL_WORLD_ID != "") # If the SIMULATION environment variable is set to 1, then only static tf publisher will start. SIMULATION = os.getenv('SIMULATION') @@ -57,6 +59,20 @@ def generate_launch_description(): rplidar_frame = DRONE_DEVICE_ID + "/rplidar" # the same definition is in static_tf_launch.py file garmin_frame = DRONE_DEVICE_ID + "/garmin" # the same definition is in static_tf_launch.py file + params=[ + pkg_share_path + '/config/params.yaml', + {"frame_id": rplidar_frame}, + ] + + if HITL_WORLD_ID is not None: + params.append({"sim_world_model": HITL_WORLD_ID + '/' + DRONE_DEVICE_ID}) + params.append({"channel_type": 'sim'}) + params.append({"scan_mode": ''}) + + ld.add_action( + LogInfo(msg='--- HITL CONFIGURATION ---', condition=IfCondition(PythonExpression([str(hitl_world_id_expr)]))), + ), + # simulation ld.add_action( LogInfo(msg='--- SIMULATION CONFIGURATION ---', condition=IfCondition(PythonExpression([str(simulation_mode)]))), @@ -73,10 +89,7 @@ def generate_launch_description(): ('topic_filtered_out', filtered_name), ('topic_raw_out', raw_name), ], - parameters = [ - pkg_share_path + '/config/params.yaml', - {"frame_id": rplidar_frame}, - ], + parameters=params, output = 'screen', ), ), diff --git a/launch/standalone_rplidar_launch.py b/launch/standalone_rplidar_launch.py index 1dbf9c31..a19429fa 100755 --- a/launch/standalone_rplidar_launch.py +++ b/launch/standalone_rplidar_launch.py @@ -1,9 +1,22 @@ from ament_index_python.packages import get_package_share_directory from launch_ros.actions import Node -from os import path +from os import path, getenv from launch import LaunchDescription def generate_launch_description(): + drone_device_id = getenv('DRONE_DEVICE_ID') + hitl_world_id = getenv('HITL_WORLD_ID', default=None) + + params=[ + path.join(get_package_share_directory('rplidar_ros2'), 'config', + 'params.yaml'), + ] + + if hitl_world_id is not None: + params.append({"sim_world_model": hitl_world_id + '/' + drone_device_id}) + params.append({"channel_type": 'sim'}) + params.append({"scan_mode": ''}) + return LaunchDescription([ Node( name='rplidar', @@ -14,10 +27,7 @@ def generate_launch_description(): ('topic_filtered_out', '~/scan_filtered'), ('topic_raw_out', '~/scan'), ], - parameters=[ - path.join(get_package_share_directory('rplidar_ros2'), 'config', - 'params.yaml'), - ], + parameters=params, output='screen', ), ]) diff --git a/sdk/include/rplidar_driver.h b/sdk/include/rplidar_driver.h index ddadd026..fe4faa70 100644 --- a/sdk/include/rplidar_driver.h +++ b/sdk/include/rplidar_driver.h @@ -63,6 +63,7 @@ struct RplidarScanMode { enum { DRIVER_TYPE_SERIALPORT = 0x0, DRIVER_TYPE_TCP = 0x1, + DRIVER_TYPE_SIM = 0x2, }; class ChannelDevice diff --git a/sdk/src/rplidar_driver.cpp b/sdk/src/rplidar_driver.cpp index e9f2ef43..b1217197 100644 --- a/sdk/src/rplidar_driver.cpp +++ b/sdk/src/rplidar_driver.cpp @@ -44,6 +44,11 @@ #include "rplidar_driver_impl.h" #include "rplidar_driver_serial.h" #include "rplidar_driver_TCP.h" +#ifdef HITL_SUPPORT + #include "rplidar_driver_sim.h" + #include + #include +#endif #include @@ -89,6 +94,10 @@ RPlidarDriver * RPlidarDriver::CreateDriver(_u32 drivertype) return new RPlidarDriverSerial(); case DRIVER_TYPE_TCP: return new RPlidarDriverTCP(); +#ifdef HITL_SUPPORT + case DRIVER_TYPE_SIM: + return new RPlidarDriverSim(); +#endif default: return NULL; } @@ -2260,4 +2269,138 @@ u_result RPlidarDriverTCP::connect(const char * ipStr, _u32 port, _u32) return RESULT_OK; } +/* HITL SIM DRIVER */ + +#ifdef HITL_SUPPORT + +RPlidarDriverSim::RPlidarDriverSim() + : _scanReady{false} +{ +} + +RPlidarDriverSim::~RPlidarDriverSim() +{ + _isConnected = false; +} + +u_result RPlidarDriverSim::connect(const char * worldSlashModel, _u32, _u32) +{ + if (_isConnected) return RESULT_ALREADY_DONE; + + std::stringstream wsm(worldSlashModel); + std::string word; + std::vector wm_list; + while ( std::getline(wsm, word, '/')) + { + wm_list.push_back(word); + } + if (wm_list.size() < 2) { + return RESULT_INVALID_DATA; + } + + _worldName = wm_list[0]; + _modelName = wm_list[1]; + + std::string scan_topic = "/world/" + _worldName + "/model/" + _modelName + "/link/2d_scanner_link/sensor/rplidar/scan"; + + if (!_node.Subscribe(scan_topic, &RPlidarDriverSim::_scanCallback, this)) { + fprintf(stderr, "[RPlidarDriverSim::connect] - ERROR: failed to subscribe to %s", scan_topic.c_str()); + return RESULT_INVALID_DATA; + } + + _isConnected = true; + + return RESULT_OK; +} + +void RPlidarDriverSim::_scanCallback(const gz::msgs::LaserScan &scan) +{ + // Store scan results + rp::hal::AutoLocker l(_lock); + _scanInput = scan; + _scanReady = true; +} + +/* Override default implementations for HITL */ +u_result RPlidarDriverSim::reset(_u32) {return RESULT_OK;} +u_result RPlidarDriverSim::clearNetSerialRxCache() {return RESULT_OK;} +u_result RPlidarDriverSim::getAllSupportedScanModes(std::vector&, _u32) {return RESULT_OPERATION_NOT_SUPPORT;} +u_result RPlidarDriverSim::getTypicalScanMode(_u16&, _u32) {return RESULT_OPERATION_NOT_SUPPORT;} +u_result RPlidarDriverSim::startScanExpress(bool, _u16, _u32, RplidarScanMode*, _u32) {return RESULT_OK;} +u_result RPlidarDriverSim::setMotorPWM(_u16) {return RESULT_OK;} +u_result RPlidarDriverSim::startMotor() {return RESULT_OK;} +u_result RPlidarDriverSim::stopMotor() {return RESULT_OK;} +u_result RPlidarDriverSim::checkMotorCtrlSupport(bool & support, _u32) {support=false; return RESULT_OK;} +u_result RPlidarDriverSim::getFrequency(const RplidarScanMode&, size_t, float &) {return RESULT_OPERATION_NOT_SUPPORT;} +u_result RPlidarDriverSim::startScanNormal(bool, _u32) {return RESULT_OK;} +u_result RPlidarDriverSim::stop(_u32) {return RESULT_OK;} + +u_result RPlidarDriverSim::getHealth(rplidar_response_device_health_t & healthinfo, _u32) +{ + healthinfo.status = RPLIDAR_STATUS_OK; + healthinfo.error_code = 0; + return RESULT_OK; +} + +u_result RPlidarDriverSim::getDeviceInfo(rplidar_response_device_info_t & info, _u32) +{ + info.model = 1; + info.firmware_version = 0x0100; + info.hardware_version = 0x01; + for (int i=0; i<16; i++) info.serialnum[i] = 0; + info.serialnum[0] = 0x0c; + info.serialnum[1] = 0x0a; + info.serialnum[2] = 0x0f; + info.serialnum[3] = 0x0f; + info.serialnum[4] = 0x0e; + info.serialnum[5] = 0x0e; + return RESULT_OK; +} + +u_result RPlidarDriverSim::startScan(bool, bool, _u32, RplidarScanMode* outUsedScanMode) +{ + outUsedScanMode->id = RPLIDAR_CONF_SCAN_COMMAND_STD; + outUsedScanMode->us_per_sample = 7200.0f / 1e6; + outUsedScanMode->max_distance = 14.0f; + outUsedScanMode->ans_type = RPLIDAR_ANS_TYPE_MEASUREMENT; + strcpy(outUsedScanMode->scan_mode, "Standard"); + + return RESULT_OK; +} + +u_result RPlidarDriverSim::grabScanDataHq(rplidar_response_measurement_node_hq_t *, size_t & count, _u32 timeout_ms) +{ + uint32_t time_count_us = 0; + while (!_scanReady) { + usleep(100); + time_count_us += 100; + if (timeout_ms < time_count_us / 1000) { + return RESULT_OPERATION_TIMEOUT; + } + } + rp::hal::AutoLocker l(_lock); + _scanReady = false; + _scanOutput = _scanInput; + count = _scanInput.count(); + return RESULT_OK; +} +u_result RPlidarDriverSim::ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count) +{ + for (size_t i = 0; i < count; i++) { + auto scan_idx = count-1 - i; + if ( isinf(_scanOutput.ranges(scan_idx)) ) { + nodebuffer[i].dist_mm_q2 = 0; + } else { + nodebuffer[i].dist_mm_q2 = (uint32_t) (_scanOutput.ranges(scan_idx) * 4000.0f); + } + nodebuffer[i].quality = ((uint8_t) _scanOutput.intensities(scan_idx)) << 2; + //std::cout << "nodebuf: " << i << ": " << nodebuffer[i].dist_mm_q2 << "\n"; + } + return RESULT_OK; +} + +u_result RPlidarDriverSim::getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t *, size_t &) {return RESULT_OK;} + +#endif /* HITL_SUPPORT */ + }}} diff --git a/sdk/src/rplidar_driver_sim.h b/sdk/src/rplidar_driver_sim.h new file mode 100644 index 00000000..22e57898 --- /dev/null +++ b/sdk/src/rplidar_driver_sim.h @@ -0,0 +1,82 @@ +/* + * RPLIDAR SDK + * + * Copyright (c) 2009 - 2014 RoboPeak Team + * http://www.robopeak.com + * Copyright (c) 2014 - 2019 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. + * + */ + +#pragma once + +#include +#include + +namespace rp { namespace standalone{ namespace rplidar { + +class RPlidarDriverSim : public RPlidarDriverImplCommon +{ +public: + + RPlidarDriverSim(); + virtual ~RPlidarDriverSim(); + virtual u_result connect(const char * worldSlashModel, _u32, _u32 flag = 0); + virtual void disconnect() {}; + + virtual u_result reset(_u32); + virtual u_result clearNetSerialRxCache(); + virtual u_result getAllSupportedScanModes(std::vector& outModes, _u32 timeoutInMs = DEFAULT_TIMEOUT); + virtual u_result getTypicalScanMode(_u16& outMode, _u32 timeoutInMs = DEFAULT_TIMEOUT); + virtual u_result startScan(bool force, bool useTypicalScan, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL); + virtual u_result startScanExpress(bool force, _u16 scanMode, _u32 options = 0, RplidarScanMode* outUsedScanMode = NULL, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result getHealth(rplidar_response_device_health_t & health, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result getDeviceInfo(rplidar_response_device_info_t & info, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result setMotorPWM(_u16 pwm); + virtual u_result startMotor(); + virtual u_result stopMotor(); + virtual u_result checkMotorCtrlSupport(bool & support, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result getFrequency(const RplidarScanMode& scanMode, size_t count, float & frequency); + virtual u_result startScanNormal(bool force, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result stop(_u32 timeout = DEFAULT_TIMEOUT); + virtual u_result grabScanDataHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count, _u32 timeout = DEFAULT_TIMEOUT); + virtual u_result ascendScanData(rplidar_response_measurement_node_hq_t * nodebuffer, size_t count); + virtual u_result getScanDataWithIntervalHq(rplidar_response_measurement_node_hq_t * nodebuffer, size_t & count); + +private: + void _scanCallback(const gz::msgs::LaserScan &scan); + + gz::transport::Node _node; + std::string _worldName; + std::string _modelName; + + gz::msgs::LaserScan _scanInput; + gz::msgs::LaserScan _scanOutput; + bool _scanReady; +}; + +}}} \ No newline at end of file diff --git a/src/RPLidarNode.cpp b/src/RPLidarNode.cpp index 34abeb65..d2ddcae2 100644 --- a/src/RPLidarNode.cpp +++ b/src/RPLidarNode.cpp @@ -47,6 +47,7 @@ RPLidarNode::RPLidarNode(const rclcpp::NodeOptions& options) : rclcpp::Node("rpl 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_sim_world_model = this->declare_parameter("sim_world_model", ""); 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")); @@ -80,6 +81,9 @@ RPLidarNode::RPLidarNode(const rclcpp::NodeOptions& options) : rclcpp::Node("rpl /* initialize SDK */ if (m_channel_type == "tcp") { m_driver = RPLidarDriverUPtr(RPLidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_TCP)); + } else if (m_channel_type == "sim") { + RCLCPP_INFO(logger, "Starting SIM driver.."); + m_driver = RPLidarDriverUPtr(RPLidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SIM)); } else { m_driver = RPLidarDriverUPtr(RPLidarDriver::CreateDriver(rp::standalone::rplidar::DRIVER_TYPE_SERIALPORT)); } @@ -97,7 +101,13 @@ RPLidarNode::RPLidarNode(const rclcpp::NodeOptions& options) : rclcpp::Node("rpl error_string << "Cannot bind to the specified TCP host: " << m_tcp_ip << ":" << m_tcp_port; throw std::runtime_error(error_string.str()); } - + } else if (m_channel_type == "sim") { + // make connection... + if (IS_FAIL(m_driver->connect(m_sim_world_model.c_str(), 0))) { + std::stringstream error_string; + error_string << "Cannot connect to simulation - world/model: " << m_sim_world_model; + 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))) { @@ -144,6 +154,7 @@ RPLidarNode::~RPLidarNode() void RPLidarNode::publish_scan(const double scan_time, const ResponseNodeArray& nodes, size_t node_count) { + const auto& logger = this->get_logger(); sensor_msgs::msg::LaserScan scan_msg; scan_count->Increment();