239 lines
9.0 KiB
C++
239 lines
9.0 KiB
C++
/*********************************************************************************************************************
|
|
Copyright (c) 2020 RoboSense
|
|
All rights reserved
|
|
|
|
By downloading, copying, installing or using the software you agree to this license. If you do not agree to this
|
|
license, do not download, install, copy or use the software.
|
|
|
|
License Agreement
|
|
For RoboSense LiDAR SDK Library
|
|
(3-clause BSD License)
|
|
|
|
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.
|
|
|
|
3. Neither the names of the RoboSense, nor Suteng Innovation Technology, nor the names of other contributors may be used
|
|
to endorse or promote products derived from this software without specific prior written permission.
|
|
|
|
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 OWNER 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 <rs_driver/api/lidar_driver.hpp>
|
|
|
|
#ifdef ENABLE_PCL_POINTCLOUD
|
|
#include <rs_driver/msg/pcl_point_cloud_msg.hpp>
|
|
#else
|
|
#include <rs_driver/msg/point_cloud_msg.hpp>
|
|
#endif
|
|
|
|
//#define ORDERLY_EXIT
|
|
|
|
// Define the macro: 1 to enable IMU parsing, 0 to disable IMU parsing
|
|
#define ENABLE_IMU_PARSE 1
|
|
|
|
typedef PointXYZI PointT;
|
|
typedef PointCloudT<PointT> PointCloudMsg;
|
|
|
|
using namespace robosense::lidar;
|
|
|
|
SyncQueue<std::shared_ptr<PointCloudMsg>> free_cloud_queue;
|
|
SyncQueue<std::shared_ptr<PointCloudMsg>> stuffed_cloud_queue;
|
|
|
|
SyncQueue<std::shared_ptr<ImuData>> free_imu_data_queue;
|
|
SyncQueue<std::shared_ptr<ImuData>> stuffed_imu_data_queue;
|
|
|
|
//
|
|
// @brief point cloud callback function. The caller should register it to the lidar driver.
|
|
// Via this fucntion, the driver gets an free/unused point cloud message from the caller.
|
|
// @param msg The free/unused point cloud message.
|
|
//
|
|
std::shared_ptr<PointCloudMsg> driverGetPointCloudFromCallerCallback(void)
|
|
{
|
|
// Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver,
|
|
// so please DO NOT do time-consuming task here.
|
|
std::shared_ptr<PointCloudMsg> msg = free_cloud_queue.pop();
|
|
if (msg.get() != NULL)
|
|
{
|
|
return msg;
|
|
}
|
|
|
|
return std::make_shared<PointCloudMsg>();
|
|
}
|
|
|
|
//
|
|
// @brief point cloud callback function. The caller should register it to the lidar driver.
|
|
// Via this function, the driver gets/returns a stuffed point cloud message to the caller.
|
|
// @param msg The stuffed point cloud message.
|
|
//
|
|
void driverReturnPointCloudToCallerCallback(std::shared_ptr<PointCloudMsg> msg)
|
|
{
|
|
// Note: This callback function runs in the packet-parsing/point-cloud-constructing thread of the driver,
|
|
// so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processCloud()
|
|
// below)
|
|
stuffed_cloud_queue.push(msg);
|
|
}
|
|
|
|
//
|
|
// @brief IMU data callback function. The caller should register it to the lidar driver.
|
|
// Via this function, the driver gets/returns a stuffed IMU data message to the caller.
|
|
// @param msg The stuffed IMU data message.
|
|
//
|
|
std::shared_ptr<ImuData> driverGetIMUDataFromCallerCallback(void)
|
|
{
|
|
// Note: This callback function runs in the packet-parsing/imu-data-constructing thread of the driver,
|
|
// so please DO NOT do time-consuming task here.
|
|
std::shared_ptr<ImuData> msg = free_imu_data_queue.pop();
|
|
if (msg.get() != NULL)
|
|
{
|
|
return msg;
|
|
}
|
|
|
|
return std::make_shared<ImuData>();
|
|
}
|
|
|
|
//
|
|
// @brief IMU data callback function. The caller should register it to the lidar driver.
|
|
// Via this function, the driver gets/returns a stuffed IMU data message to the caller.
|
|
// @param msg The stuffed IMU data message.
|
|
//
|
|
void driverReturnImuDataToCallerCallback(const std::shared_ptr<ImuData>& msg)
|
|
{
|
|
// Note: This callback function runs in the packet-parsing/imu-data-constructing thread of the driver,
|
|
// so please DO NOT do time-consuming task here. Instead, process it in caller's own thread. (see processImuData() below)
|
|
stuffed_imu_data_queue.push(msg);
|
|
}
|
|
|
|
|
|
bool to_exit_process = false;
|
|
void processImuData(void)
|
|
{
|
|
uint32_t imu_cnt = 0;
|
|
while (!to_exit_process)
|
|
{
|
|
std::shared_ptr<ImuData> msg = stuffed_imu_data_queue.popWait();
|
|
if (msg.get() == NULL)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Well, it is time to process the IMU data msg, even it is time-consuming.
|
|
|
|
imu_cnt++;
|
|
#if 0
|
|
RS_MSG << "msg: " << imu_cnt << " imu data ts: " <<std::dec<<std::to_string(msg->timestamp) << RS_REND;
|
|
RS_DEBUG <<"imu data: " << " , linear_a_x" << msg->linear_acceleration_x
|
|
<< " , linear_a_y " << msg->linear_acceleration_y << " , linear_a_z" << msg->linear_acceleration_z
|
|
<< " , angular_v_x " << msg->angular_velocity_x << " , angular_v_y" << msg->angular_velocity_y
|
|
<< " , angular_v_z" <<msg->angular_velocity_z << RS_REND;
|
|
#endif
|
|
|
|
free_imu_data_queue.push(msg);
|
|
}
|
|
|
|
}
|
|
//
|
|
// @brief exception callback function. The caller should register it to the lidar driver.
|
|
// Via this function, the driver inform the caller that something happens.
|
|
// @param code The error code to represent the error/warning/information
|
|
//
|
|
void exceptionCallback(const Error& code)
|
|
{
|
|
// Note: This callback function runs in the packet-receving and packet-parsing/point-cloud_constructing thread of the
|
|
// driver,
|
|
// so please DO NOT do time-consuming task here.
|
|
RS_WARNING << code.toString() << RS_REND;
|
|
}
|
|
|
|
|
|
void processCloud(void)
|
|
{
|
|
while (!to_exit_process)
|
|
{
|
|
std::shared_ptr<PointCloudMsg> msg = stuffed_cloud_queue.popWait();
|
|
if (msg.get() == NULL)
|
|
{
|
|
continue;
|
|
}
|
|
|
|
// Well, it is time to process the point cloud msg, even it is time-consuming.
|
|
RS_MSG << "msg: " << msg->seq << " point cloud size: " << msg->points.size() << RS_REND;
|
|
|
|
#if 0
|
|
for (auto it = msg->points.begin(); it != msg->points.end(); it++)
|
|
{
|
|
std::cout << std::fixed << std::setprecision(8) << "(" << it->x << ", " << it->y << ", " << it->z << ", "
|
|
<< (int)it->intensity << ", " << it->timestamp << ")" << std::endl;
|
|
}
|
|
#endif
|
|
|
|
free_cloud_queue.push(msg);
|
|
|
|
|
|
}
|
|
}
|
|
|
|
int main(int argc, char* argv[])
|
|
{
|
|
RS_TITLE << "------------------------------------------------------" << RS_REND;
|
|
RS_TITLE << " RS_Driver Core Version: v" << getDriverVersion() << RS_REND;
|
|
RS_TITLE << "------------------------------------------------------" << RS_REND;
|
|
|
|
RSDriverParam param; ///< Create a parameter object
|
|
param.input_type = InputType::ONLINE_LIDAR;
|
|
param.input_param.msop_port = 6699; ///< Set the lidar msop port number, the default is 6699
|
|
param.input_param.difop_port = 7788; ///< Set the lidar difop port number, the default is 7788
|
|
#if ENABLE_IMU_PARSE
|
|
param.input_param.imu_port = 6688; ///< Set the lidar imu port number, the default is 0
|
|
#endif
|
|
param.lidar_type = LidarType::RSAIRY; ///< Set the lidar type. Make sure this type is correct
|
|
param.print();
|
|
|
|
LidarDriver<PointCloudMsg> driver; ///< Declare the driver object
|
|
driver.regPointCloudCallback(driverGetPointCloudFromCallerCallback, driverReturnPointCloudToCallerCallback); ///< Register the point cloud callback functions
|
|
driver.regExceptionCallback(exceptionCallback); ///< Register the exception callback function
|
|
#if ENABLE_IMU_PARSE
|
|
driver.regImuDataCallback(driverGetIMUDataFromCallerCallback, driverReturnImuDataToCallerCallback);
|
|
#endif
|
|
|
|
if (!driver.init(param)) ///< Call the init function
|
|
{
|
|
RS_ERROR << "Driver Initialize Error..." << RS_REND;
|
|
return -1;
|
|
}
|
|
|
|
std::thread cloud_handle_thread = std::thread(processCloud);
|
|
#if ENABLE_IMU_PARSE
|
|
std::thread imuData_handle_thread = std::thread(processImuData);
|
|
#endif
|
|
driver.start(); ///< The driver thread will start
|
|
RS_DEBUG << "RoboSense Lidar-Driver Linux online demo start......" << RS_REND;
|
|
|
|
#ifdef ORDERLY_EXIT
|
|
std::this_thread::sleep_for(std::chrono::seconds(10));
|
|
|
|
driver.stop();
|
|
|
|
to_exit_process = true;
|
|
cloud_handle_thread.join();
|
|
#else
|
|
while (true)
|
|
{
|
|
std::this_thread::sleep_for(std::chrono::seconds(1));
|
|
}
|
|
#endif
|
|
|
|
return 0;
|
|
}
|