diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/CMakeLists.txt b/unitree_lidar_ros/src/unitree_lidar_ros/CMakeLists.txt index 3aa9749..a40fc8c 100755 --- a/unitree_lidar_ros/src/unitree_lidar_ros/CMakeLists.txt +++ b/unitree_lidar_ros/src/unitree_lidar_ros/CMakeLists.txt @@ -56,5 +56,6 @@ set(EXTRA_LIBS ${PCL_LIBRARIES} libunitree_lidar_sdk.a ) + add_executable(unitree_lidar_ros_node src/unitree_lidar_ros_node.cpp) -target_link_libraries(unitree_lidar_ros_node ${EXTRA_LIBS}) +target_link_libraries(unitree_lidar_ros_node ${EXTRA_LIBS}) \ No newline at end of file diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/config/config.yaml b/unitree_lidar_ros/src/unitree_lidar_ros/config/config.yaml index 5599ebc..4c65498 100755 --- a/unitree_lidar_ros/src/unitree_lidar_ros/config/config.yaml +++ b/unitree_lidar_ros/src/unitree_lidar_ros/config/config.yaml @@ -2,9 +2,18 @@ # lidar sdk configuration #################################################### +# Initialization Type +initialize_type: 1 # 1-Serial, 2-UDP + # Serial Port port: "/dev/ttyUSB0" # default: "/dev/ttyUSB0" +# UDP Port +lidar_port: 6101 +lidar_ip: "10.10.10.10" +local_port: 6201 +local_ip: "10.10.10.100" + # Calibration rotate_yaw_bias: 0 # default: 0 in degree range_scale: 0.001 # default: 0.001 in meters @@ -15,7 +24,7 @@ range_min: 0.0 # default: 0 in meters # PointCloud cloud_frame: "unilidar_lidar" # default: "unilidar_lidar" cloud_topic: "unilidar/cloud" # default: "unilidar/cloud" -cloud_scan_num: 18 # default: 18 +cloud_scan_num: 2 # default: 18 # IMU imu_frame: "unilidar_imu" # default: "unilidar_imu" diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros.h b/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros.h index b002a2a..1dd9cda 100644 --- a/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros.h +++ b/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros.h @@ -86,13 +86,26 @@ class UnitreeLidarSDKNode{ std::string imu_frame_; std::string imu_topic_; + int initialize_type_; + int local_port_; + std::string local_ip_; + int lidar_port_; + std::string lidar_ip_; + public: UnitreeLidarSDKNode(ros::NodeHandle nh){ - // load config parameters + // Load config parameters + nh.param("/unitree_lidar_ros_node/initialize_type", initialize_type_, 1); + nh.param("/unitree_lidar_ros_node/port", port_, std::string("/dev/ttyUSB0")); + nh.param("/unitree_lidar_ros_node/lidar_port", lidar_port_, 6101); + nh.param("/unitree_lidar_ros_node/lidar_ip", lidar_ip_, std::string("10.10.10.10")); + nh.param("/unitree_lidar_ros_node/local_port", local_port_, 6201); + nh.param("/unitree_lidar_ros_node/local_ip", local_ip_, std::string("10.10.10.100")); + nh.param("/unitree_lidar_ros_node/rotate_yaw_bias", rotate_yaw_bias_, 0.0); nh.param("/unitree_lidar_ros_node/range_scale", range_scale_, 0.001); nh.param("/unitree_lidar_ros_node/range_bias", range_bias_, 0.0); @@ -107,8 +120,18 @@ class UnitreeLidarSDKNode{ // Initialize UnitreeLidarReader lsdk_ = createUnitreeLidarReader(); - lsdk_->initialize(cloud_scan_num_, port_, 2000000, rotate_yaw_bias_, + + if (initialize_type_ == 1){ + lsdk_->initialize(cloud_scan_num_, port_, 2000000, rotate_yaw_bias_, range_scale_, range_bias_, range_max_, range_min_); + } + else if (initialize_type_ == 2){ + lsdk_->initializeUDP(cloud_scan_num_, lidar_port_, lidar_ip_, local_port_, local_ip_, + rotate_yaw_bias_, range_scale_, range_bias_, range_max_, range_min_); + } + + + lsdk_->setLidarWorkingMode(NORMAL); // ROS nh_ = nh; @@ -124,7 +147,7 @@ class UnitreeLidarSDKNode{ */ bool run(){ MessageType result = lsdk_->runParse(); - static pcl::PointCloud::Ptr cloudOut( new pcl::PointCloud() ); + pcl::PointCloud::Ptr cloudOut( new pcl::PointCloud() ); if (result == IMU){ auto & imu = lsdk_->getIMU(); diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros_multiple.h b/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros_multiple.h new file mode 100644 index 0000000..16c01f5 --- /dev/null +++ b/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros_multiple.h @@ -0,0 +1,233 @@ +/********************************************************************** + Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. +***********************************************************************/ + +#pragma once + +// ROS +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// PCL +#include + +// Eigen +#include +#include + +#include "unitree_lidar_sdk_pcl.h" + +/** + * @brief Publish a pointcloud + * + * @param thisPub + * @param thisCloud + * @param thisStamp + * @param thisFrame + * @return sensor_msgs::PointCloud2 + */ +inline sensor_msgs::PointCloud2 publishCloud( + ros::Publisher *thisPub, + pcl::PointCloud::Ptr thisCloud, + ros::Time thisStamp, + std::string thisFrame) +{ + sensor_msgs::PointCloud2 tempCloud; + pcl::toROSMsg(*thisCloud, tempCloud); + tempCloud.header.stamp = thisStamp; + tempCloud.header.frame_id = thisFrame; + if (thisPub->getNumSubscribers() != 0) + thisPub->publish(tempCloud); + return tempCloud; +} + +/** + * @brief Unitree Lidar SDK Node + */ +class UnitreeLidarSDKNodeMultiple{ +protected: + + // ROS + ros::NodeHandle nh_; + ros::Publisher pub_pointcloud_raw_; + ros::Publisher pub_imu_; + tf::TransformBroadcaster tfbc1_; + + // Unitree Lidar Reader + UnitreeLidarReader* lsdk_; + UnitreeLidarReader* lsdk2_; + + // Config params + std::string port_; + + double rotate_yaw_bias_; + double range_scale_; + double range_bias_; + double range_max_; + double range_min_; + + std::string cloud_frame_; + std::string cloud_topic_; + int cloud_scan_num_; + + std::string imu_frame_; + std::string imu_topic_; + + int initialize_type_; + int local_port_; + std::string local_ip_; + int lidar_port_; + std::string lidar_ip_; + +public: + + UnitreeLidarSDKNodeMultiple(ros::NodeHandle nh){ + + // Load config parameters + nh.param("/unitree_lidar_ros_node/initialize_type", initialize_type_, 1); + + nh.param("/unitree_lidar_ros_node/port", port_, std::string("/dev/ttyUSB0")); + + nh.param("/unitree_lidar_ros_node/lidar_port", lidar_port_, 6101); + nh.param("/unitree_lidar_ros_node/lidar_ip", lidar_ip_, std::string("10.10.10.10")); + + nh.param("/unitree_lidar_ros_node/local_port", local_port_, 6201); + nh.param("/unitree_lidar_ros_node/local_ip", local_ip_, std::string("10.10.10.100")); + + nh.param("/unitree_lidar_ros_node/rotate_yaw_bias", rotate_yaw_bias_, 0.0); + nh.param("/unitree_lidar_ros_node/range_scale", range_scale_, 0.001); + nh.param("/unitree_lidar_ros_node/range_bias", range_bias_, 0.0); + nh.param("/unitree_lidar_ros_node/range_max", range_max_, 50.0); + nh.param("/unitree_lidar_ros_node/range_min", range_min_, 0.0); + + nh.param("/unitree_lidar_ros_node/cloud_frame", cloud_frame_, std::string("unilidar_lidar")); + nh.param("/unitree_lidar_ros_node/cloud_topic", cloud_topic_, std::string("unilidar/cloud")); + nh.param("/unitree_lidar_ros_node/cloud_scan_num", cloud_scan_num_, 18); + nh.param("/unitree_lidar_ros_node/imu_frame", imu_frame_, std::string("unilidar_imu")); + nh.param("/unitree_lidar_ros_node/imu_topic", imu_topic_, std::string("unilidar/imu")); + + // Initialize UnitreeLidarReader + lsdk_ = createUnitreeLidarReader(); + lsdk2_ = createUnitreeLidarReader(); + + if (initialize_type_ == 1){ + lsdk_->initialize(cloud_scan_num_, port_, 2000000, rotate_yaw_bias_, + range_scale_, range_bias_, range_max_, range_min_); + } + else if (initialize_type_ == 2){ + lsdk_->initializeUDP(cloud_scan_num_, lidar_port_, lidar_ip_, local_port_, local_ip_, + rotate_yaw_bias_, range_scale_, range_bias_, range_max_, range_min_); + lsdk2_->initializeUDP(cloud_scan_num_, lidar_port_+1, lidar_ip_, local_port_+1, local_ip_, + rotate_yaw_bias_, range_scale_, range_bias_, range_max_, range_min_); + } + + lsdk_->setLidarWorkingMode(NORMAL); + sleep(1); + lsdk2_->setLidarWorkingMode(NORMAL); + sleep(1); + + // ROS + nh_ = nh; + pub_pointcloud_raw_ = nh.advertise (cloud_topic_, 100); + pub_imu_ = nh.advertise (imu_topic_, 1000); + } + + ~UnitreeLidarSDKNodeMultiple(){ + } + + /** + * @brief Run once + */ + bool run(){ + MessageType result = lsdk_->runParse(); + pcl::PointCloud::Ptr cloudOut( new pcl::PointCloud() ); + + if (result == IMU){ + auto & imu = lsdk_->getIMU(); + sensor_msgs::Imu imuMsg; + imuMsg.header.frame_id = imu_frame_; + imuMsg.header.stamp = ros::Time::now().fromSec(imu.stamp); + + imuMsg.orientation.x = imu.quaternion[0]; + imuMsg.orientation.y = imu.quaternion[1]; + imuMsg.orientation.z = imu.quaternion[2]; + imuMsg.orientation.w = imu.quaternion[3]; + + imuMsg.angular_velocity.x = imu.angular_velocity[0]; + imuMsg.angular_velocity.y = imu.angular_velocity[1]; + imuMsg.angular_velocity.z = imu.angular_velocity[2]; + + imuMsg.linear_acceleration.x = imu.linear_acceleration[0]; + imuMsg.linear_acceleration.y = imu.linear_acceleration[1]; + imuMsg.linear_acceleration.z = imu.linear_acceleration[2]; + + pub_imu_.publish(imuMsg); + + return true; + } + else if (result == POINTCLOUD){ + auto &cloud = lsdk_->getCloud(); + transformUnitreeCloudToPCL(cloud, cloudOut); + publishCloud(&pub_pointcloud_raw_, cloudOut, ros::Time::now().fromSec(cloud.stamp), cloud_frame_); + + return true; + }else{ + return false; + } + + } + + bool run2(){ + MessageType result = lsdk2_->runParse(); + pcl::PointCloud::Ptr cloudOut( new pcl::PointCloud() ); + + if (result == IMU){ + auto & imu = lsdk2_->getIMU(); + sensor_msgs::Imu imuMsg; + imuMsg.header.frame_id = imu_frame_; + imuMsg.header.stamp = ros::Time::now().fromSec(imu.stamp); + + imuMsg.orientation.x = imu.quaternion[0]; + imuMsg.orientation.y = imu.quaternion[1]; + imuMsg.orientation.z = imu.quaternion[2]; + imuMsg.orientation.w = imu.quaternion[3]; + + imuMsg.angular_velocity.x = imu.angular_velocity[0]; + imuMsg.angular_velocity.y = imu.angular_velocity[1]; + imuMsg.angular_velocity.z = imu.angular_velocity[2]; + + imuMsg.linear_acceleration.x = imu.linear_acceleration[0]; + imuMsg.linear_acceleration.y = imu.linear_acceleration[1]; + imuMsg.linear_acceleration.z = imu.linear_acceleration[2]; + + pub_imu_.publish(imuMsg); + + return true; + } + else if (result == POINTCLOUD){ + auto &cloud = lsdk2_->getCloud(); + transformUnitreeCloudToPCL(cloud, cloudOut); + publishCloud(&pub_pointcloud_raw_, cloudOut, ros::Time::now().fromSec(cloud.stamp), cloud_frame_); + + return true; + }else{ + return false; + } + + } + +}; \ No newline at end of file diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/launch/run_multiple.launch b/unitree_lidar_ros/src/unitree_lidar_ros/launch/run_multiple.launch new file mode 100755 index 0000000..a9e1d7c --- /dev/null +++ b/unitree_lidar_ros/src/unitree_lidar_ros/launch/run_multiple.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/launch/run_without_rviz.launch b/unitree_lidar_ros/src/unitree_lidar_ros/launch/run_without_rviz.launch new file mode 100755 index 0000000..d3be6f1 --- /dev/null +++ b/unitree_lidar_ros/src/unitree_lidar_ros/launch/run_without_rviz.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/unitree_lidar_ros/src/unitree_lidar_ros/src/unitree_lidar_ros_node.cpp b/unitree_lidar_ros/src/unitree_lidar_ros/src/unitree_lidar_ros_node.cpp index 28534ff..47fec74 100755 --- a/unitree_lidar_ros/src/unitree_lidar_ros/src/unitree_lidar_ros_node.cpp +++ b/unitree_lidar_ros/src/unitree_lidar_ros/src/unitree_lidar_ros_node.cpp @@ -23,4 +23,4 @@ int main(int argc, char **argv) } return 0; -} +} \ No newline at end of file diff --git a/unitree_lidar_sdk/CMakeLists.txt b/unitree_lidar_sdk/CMakeLists.txt index 4982686..8f6158d 100644 --- a/unitree_lidar_sdk/CMakeLists.txt +++ b/unitree_lidar_sdk/CMakeLists.txt @@ -18,6 +18,11 @@ add_executable(example_lidar ) target_link_libraries(example_lidar libunitree_lidar_sdk.a ) +add_executable(example_lidar_udp + examples/example_lidar_udp.cpp +) +target_link_libraries(example_lidar_udp libunitree_lidar_sdk.a ) + add_executable(unilidar_publisher_udp examples/unilidar_publisher_udp.cpp ) diff --git a/unitree_lidar_sdk/doc/HowToParsePointCloudAndIMUDataFromMavLinkMessages.md b/unitree_lidar_sdk/HowToParsePointCloudAndIMUDataFromMavLinkMessages.md similarity index 100% rename from unitree_lidar_sdk/doc/HowToParsePointCloudAndIMUDataFromMavLinkMessages.md rename to unitree_lidar_sdk/HowToParsePointCloudAndIMUDataFromMavLinkMessages.md diff --git a/unitree_lidar_sdk/README.md b/unitree_lidar_sdk/README.md index 74d4ac5..c023b85 100644 --- a/unitree_lidar_sdk/README.md +++ b/unitree_lidar_sdk/README.md @@ -174,4 +174,9 @@ If you want to parse point cloud from MavLink Messages which are acquired from s ### v1.0.11 (2023.07.27) - Add a instruction document for how to parse point cloud and imu data from mavlink messages directly. -- The document is `HowToParsePointCloudAndIMUDataFromMavLinkMessages.md` \ No newline at end of file +- The document is `HowToParsePointCloudAndIMUDataFromMavLinkMessages.md` + +### v1.0.12 (2023.09.20) +unitree_lidar_ros: +- Add support of UDP interface, which can parse original bytes from a specified UDP port and send commands to lidar ip and port. +- Update `unitree_lidar_ros` \ No newline at end of file diff --git a/unitree_lidar_sdk/examples/example_lidar_udp.cpp b/unitree_lidar_sdk/examples/example_lidar_udp.cpp new file mode 100644 index 0000000..f388302 --- /dev/null +++ b/unitree_lidar_sdk/examples/example_lidar_udp.cpp @@ -0,0 +1,155 @@ +/********************************************************************** + Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved. +***********************************************************************/ + +#include "unitree_lidar_sdk.h" + +using namespace unitree_lidar_sdk; + +int main(int argc, char *argv[]){ + + // Initialize Lidar Object + UnitreeLidarReader* lreader = createUnitreeLidarReader(); + + std::string lidar_ip = "10.10.10.10"; + std::string local_ip = "10.10.10.100"; + + unsigned short lidar_port = 6101; + unsigned short local_port = 6201; + + if (argc == 3) + { + lidar_port = std::atoi(argv[1]); + local_port = std::atoi(argv[2]); + }else{ + std::cout << "Usage: ./example_lidar_udp " << std::endl; + return 0; + } + + std::cout << "lidar_port = " << lidar_port << std::endl; + + if ( lreader->initializeUDP(18, lidar_port, lidar_ip, local_port, local_ip) ){ + printf("Unilidar initialization failed! Exit here!\n"); + exit(-1); + }else{ + printf("Unilidar initialization succeed!\n"); + } + + // Set Lidar Working Mode + printf("Set Lidar working mode to: STANDBY ... \n"); + lreader->setLidarWorkingMode(STANDBY); + sleep(1); + + printf("Set Lidar working mode to: NORMAL ... \n"); + lreader->setLidarWorkingMode(NORMAL); + sleep(1); + + printf("\n"); + + // Print Lidar Version + while(true){ + if (lreader->runParse() == VERSION){ + printf("lidar firmware version = %s\n", lreader->getVersionOfFirmware().c_str() ); + break; + } + usleep(500); + } + printf("lidar sdk version = %s\n\n", lreader->getVersionOfSDK().c_str()); + sleep(2); + + // Check lidar dirty percentange + int count_percentage = 0; + while(true){ + if( lreader->runParse() == AUXILIARY){ + printf("Dirty Percentage = %f %%\n", lreader->getDirtyPercentage()); + if (++count_percentage > 2){ + break; + } + if (lreader->getDirtyPercentage() > 10){ + printf("The protection cover is too dirty! Please clean it right now! Exit here ...\n"); + exit(0); + } + } + usleep(500); + } + printf("\n"); + sleep(2); + + // Set LED + printf("Turn on all the LED lights ...\n"); + uint8_t led_table[45]; + for (int i=0; i < 45; i++){ + led_table[i] = 0xFF; + } + lreader->setLEDDisplayMode(led_table); + sleep(2); + + printf("Turn off all the LED lights ...\n"); + for (int i=0; i < 45; i++){ + led_table[i] = 0x00; + } + lreader->setLEDDisplayMode(led_table); + sleep(2); + + printf("Set LED mode to: FORWARD_SLOW ...\n"); + lreader->setLEDDisplayMode(FORWARD_SLOW); + sleep(2); + + printf("Set LED mode to: REVERSE_SLOW ...\n"); + lreader->setLEDDisplayMode(REVERSE_SLOW); + sleep(2); + + printf("Set LED mode to: SIXSTAGE_BREATHING ...\n"); + lreader->setLEDDisplayMode(SIXSTAGE_BREATHING); + + printf("\n"); + sleep(2); + + // Parse PointCloud and IMU data + MessageType result; + std::string version; + while (true) + { + result = lreader->runParse(); // You need to call this function at least 1500Hz + + switch (result) + { + case NONE: + break; + + case IMU: + printf("An IMU msg is parsed!\n"); + printf("\tstamp = %f, id = %d\n", lreader->getIMU().stamp, lreader->getIMU().id); + printf("\tquaternion (x, y, z, w) = [%.4f, %.4f, %.4f, %.4f]\n", + lreader->getIMU().quaternion[0], lreader->getIMU().quaternion[1], + lreader->getIMU().quaternion[2], lreader->getIMU().quaternion[3]); + printf("\ttimedelay (us) = %d\n\n", lreader->getTimeDelay()); + break; + + case POINTCLOUD: + printf("A Cloud msg is parsed! \n"); + printf("\tstamp = %f, id = %d\n", lreader->getCloud().stamp, lreader->getCloud().id); + printf("\tcloud size = %ld, ringNum = %d\n", lreader->getCloud().points.size(), lreader->getCloud().ringNum); + printf("\tfirst 10 points (x,y,z,intensity,time,ring) = \n"); + for (int i = 0; i< 10; i++){ // print the first 10 points + printf("\t (%f, %f, %f, %f, %f, %d)\n", + lreader->getCloud().points[i].x, + lreader->getCloud().points[i].y, + lreader->getCloud().points[i].z, + lreader->getCloud().points[i].intensity, + lreader->getCloud().points[i].time, + lreader->getCloud().points[i].ring); + } + printf("\t ...\n"); + printf("\ttimedelay (us) = %d\n\n", lreader->getTimeDelay()); + break; + + default: + break; + } + + usleep(500); + } + + return 0; +} \ No newline at end of file diff --git a/unitree_lidar_sdk/examples/unilidar_subcriber_udp.py b/unitree_lidar_sdk/examples/unilidar_subcriber_udp.py index e06485e..c26fed0 100644 --- a/unitree_lidar_sdk/examples/unilidar_subcriber_udp.py +++ b/unitree_lidar_sdk/examples/unilidar_subcriber_udp.py @@ -49,7 +49,6 @@ def __init__(self, stamp, id, quaternion, angular_velocity, linear_acceleration) print("pointSize = " +str(pointSize) + ", scanDataSize = " + str(scanDataSize) + ", imuDataSize = " + str(imuDataSize)) while True: - xxx = UDP_PORT # Recv data data, addr = sock.recvfrom(10000) print(f"Received data from {addr[0]}:{addr[1]}") @@ -92,4 +91,4 @@ def __init__(self, stamp, id, quaternion, angular_velocity, linear_acceleration) print("\t", point.x, point.y, point.z, point.intensity, point.time, point.ring) print("\n") -sock.close() \ No newline at end of file +sock.close() diff --git a/unitree_lidar_sdk/examples/unilidar_subscriber_udp.cpp b/unitree_lidar_sdk/examples/unilidar_subscriber_udp.cpp index b3dfc89..ce7a358 100644 --- a/unitree_lidar_sdk/examples/unilidar_subscriber_udp.cpp +++ b/unitree_lidar_sdk/examples/unilidar_subscriber_udp.cpp @@ -75,7 +75,7 @@ int main(int argc, char *argv[]) imuMsg.quaternion[2], imuMsg.quaternion[3]); printf("\n"); } - else if (msgType == 102) + else if (msgType == 102) // 4+4+2896 bytes, which stands for [msgType, dataSize, data of ScanUnitree] { memcpy(&length, recv_buf + 4, 4); memcpy(&scanMsg, recv_buf + 8, sizeof(ScanUnitree)); diff --git a/unitree_lidar_sdk/include/parse_range_auxiliary_data_to_cloud.h b/unitree_lidar_sdk/include/parse_range_auxiliary_data_to_cloud.h new file mode 100644 index 0000000..6e3c880 --- /dev/null +++ b/unitree_lidar_sdk/include/parse_range_auxiliary_data_to_cloud.h @@ -0,0 +1,81 @@ +/** + * @file parse_range_auxiliary_data_to_cloud.h + * @date 2023-07-27 + */ + +#pragma once +#include "mavlink/SysMavlink/mavlink.h" +#include +#include + +/** + * @brief Parse A Range And An Auxiliary Data To A Point Cloud + * + * @param auxiliaryData + * @param rangeData + * @param scanCloud Array elements are x, y, z, intensity respectively + * @return true + * @return false + */ +bool parseRangeAuxiliaryDataToCloud( + const mavlink_ret_lidar_auxiliary_data_packet_t &auxiliaryData, + const mavlink_ret_lidar_distance_data_packet_t &rangeData, + std::vector> &scanCloud + ){ + + // check packet id match + if (auxiliaryData.packet_id != rangeData.packet_id){ + return false; + } + + // parse data + float rotate_yaw_bias = 0; + float range_scale = 0.001; + float z_bias = 0.0445; + int points_num_of_scan = 120; + float bias_laser_beam_ = auxiliaryData.b_axis_dist / 1000; + + float sin_theta = sin(auxiliaryData.theta_angle); + float cos_theta = cos(auxiliaryData.theta_angle); + float sin_ksi = sin(auxiliaryData.ksi_angle); + float cos_ksi = cos(auxiliaryData.ksi_angle); + + float pitch_cur = auxiliaryData.sys_vertical_angle_start * M_PI / 180.0; + float pitch_step = auxiliaryData.sys_vertical_angle_span * M_PI / 180.0; + float yaw_cur = (auxiliaryData.com_horizontal_angle_start + rotate_yaw_bias) * M_PI / 180.0; + float yaw_step = auxiliaryData.com_horizontal_angle_step / points_num_of_scan * M_PI / 180.0; + + uint16_t range; + float range_float; + + std::array point; + for (int i = 0, j = 0; j < points_num_of_scan; + i += 2, j += 1, + pitch_cur += pitch_step, yaw_cur += yaw_step) + { + // Calculate point range in float type + range = (rangeData.point_data[i + 1] << 8); + range = range | rangeData.point_data[i]; + if (range == 0){ + continue; + } + range_float = range_scale * range; + + // Transform this point to XYZ coordinate + float&& sin_alpha = sin(pitch_cur); + float&& cos_alpha = cos(pitch_cur); + float&& sin_beta = sin(yaw_cur); + float&& cos_beta = cos(yaw_cur); + + float&& A = (-cos_theta * sin_ksi + sin_theta * sin_alpha * cos_ksi) * range_float + bias_laser_beam_; + float&& B = cos_alpha * cos_ksi * range_float; + + point[0] = cos_beta * A - sin_beta * B; // x + point[1] = sin_beta * A + cos_beta * B; // y + point[2] = (sin_theta * sin_ksi + cos_theta * sin_alpha * cos_ksi) * range_float + z_bias; // z + point[3] = auxiliaryData.reflect_data[j]; + scanCloud.push_back(point); + } + + return true; +} \ No newline at end of file diff --git a/unitree_lidar_sdk/include/unitree_lidar_sdk.h b/unitree_lidar_sdk/include/unitree_lidar_sdk.h index 5a6f7df..265e0f4 100644 --- a/unitree_lidar_sdk/include/unitree_lidar_sdk.h +++ b/unitree_lidar_sdk/include/unitree_lidar_sdk.h @@ -145,6 +145,22 @@ class UnitreeLidarReader{ float range_min = 0 ) = 0; + /** + * @brief Initialize for UDP board + */ + virtual int initializeUDP( + uint16_t cloud_scan_num = 18, + unsigned short lidar_port = 5001, + std::string lidar_ip = "10.10.10.10", + unsigned short local_port = 5000, + std::string local_ip = "10.10.10.100", + float rotate_yaw_bias = 0, + float range_scale = 0.001, + float range_bias = 0, + float range_max = 50, + float range_min = 0 + ) = 0; + /** * @brief Try to parse a message from the serial buffer once. * @note This is the main entrance of this class diff --git a/unitree_lidar_sdk/include/unitree_lidar_sdk_config.h b/unitree_lidar_sdk/include/unitree_lidar_sdk_config.h index 54b68c1..e1064db 100644 --- a/unitree_lidar_sdk/include/unitree_lidar_sdk_config.h +++ b/unitree_lidar_sdk/include/unitree_lidar_sdk_config.h @@ -1,4 +1,4 @@ -#define unitree_lidar_sdk_VERSION "1.0.10" +#define unitree_lidar_sdk_VERSION "1.0.12" #define unitree_lidar_sdk_VERSION_MAJOR 1 #define unitree_lidar_sdk_VERSION_MINOR 0 -#define unitree_lidar_sdk_VERSION_PATCH 10 +#define unitree_lidar_sdk_VERSION_PATCH 12 diff --git a/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a b/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a index 8783cea..9028d35 100644 Binary files a/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a and b/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a differ diff --git a/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a b/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a index 22139f1..a8d04ae 100644 Binary files a/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a and b/unitree_lidar_sdk/lib/x86_64/libunitree_lidar_sdk.a differ