HITL support added

HITL support requires several gazebo packages to be installed. To disable HITL support build with cmake option 'RPLIDAR_BUILD_HITL_SUPPORT=OFF'
tiiuae · Aug 11, 2023 · 0e033e7 · 0e033e7
1 parent 920b52c
commit 0e033e7
Show file tree

Hide file tree

Showing 8 changed files with 289 additions and 10 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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 <gz/math.hh>
+  #include <gz/msgs/laserscan.pb.h>
+#endif
 
 #include <algorithm>
 
@@ -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<std::string> 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<RplidarScanMode>&, _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
@@ -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 <gz/msgs.hh>
+#include <gz/transport.hh>
+
+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<RplidarScanMode>& 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;
+};
+
+}}}