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 4c65498..f3c96f4 100755 --- a/unitree_lidar_ros/src/unitree_lidar_ros/config/config.yaml +++ b/unitree_lidar_ros/src/unitree_lidar_ros/config/config.yaml @@ -24,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: 2 # default: 18 +cloud_scan_num: 18 # default: 18 # IMU imu_frame: "unilidar_imu" # default: "unilidar_imu" 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 deleted file mode 100644 index 16c01f5..0000000 --- a/unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros_multiple.h +++ /dev/null @@ -1,233 +0,0 @@ -/********************************************************************** - 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 deleted file mode 100755 index a9e1d7c..0000000 --- a/unitree_lidar_ros/src/unitree_lidar_ros/launch/run_multiple.launch +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - 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 47fec74..aa7f05a 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 @@ -13,13 +13,13 @@ int main(int argc, char **argv) ros::NodeHandle nh; UnitreeLidarSDKNode node(nh); - ros::Rate rate(2000); + // ros::Rate rate(2000); while (nh.ok()) { ros::spinOnce(); node.run(); - rate.sleep(); + // rate.sleep(); } return 0; diff --git a/unitree_lidar_sdk/README.md b/unitree_lidar_sdk/README.md index c023b85..0d9b42c 100644 --- a/unitree_lidar_sdk/README.md +++ b/unitree_lidar_sdk/README.md @@ -177,6 +177,8 @@ If you want to parse point cloud from MavLink Messages which are acquired from s - 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 +- Update `unitree_lidar_ros` + +### v1.0.13 (2023.10.09) +- Modify the serial port reading method in `unitree_lidar_sdk`. It will block when you call `runParse()` function, waiting for the serial port to have bytes and then reading the data once. \ No newline at end of file diff --git a/unitree_lidar_sdk/examples/example_lidar.cpp b/unitree_lidar_sdk/examples/example_lidar.cpp index 582254f..a46aa1f 100644 --- a/unitree_lidar_sdk/examples/example_lidar.cpp +++ b/unitree_lidar_sdk/examples/example_lidar.cpp @@ -133,7 +133,7 @@ int main(){ break; } - usleep(500); + // usleep(500); } return 0; diff --git a/unitree_lidar_sdk/examples/example_lidar_udp.cpp b/unitree_lidar_sdk/examples/example_lidar_udp.cpp index f388302..687e85f 100644 --- a/unitree_lidar_sdk/examples/example_lidar_udp.cpp +++ b/unitree_lidar_sdk/examples/example_lidar_udp.cpp @@ -148,7 +148,7 @@ int main(int argc, char *argv[]){ break; } - usleep(500); + // usleep(500); } return 0; diff --git a/unitree_lidar_sdk/include/unitree_lidar_sdk_config.h b/unitree_lidar_sdk/include/unitree_lidar_sdk_config.h index e1064db..aae31e5 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.12" +#define unitree_lidar_sdk_VERSION "1.0.13" #define unitree_lidar_sdk_VERSION_MAJOR 1 #define unitree_lidar_sdk_VERSION_MINOR 0 -#define unitree_lidar_sdk_VERSION_PATCH 12 +#define unitree_lidar_sdk_VERSION_PATCH 13 diff --git a/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a b/unitree_lidar_sdk/lib/aarch64/libunitree_lidar_sdk.a index 9028d35..2ba5afa 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 a8d04ae..63369fc 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