Update to v1.0.12

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})

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" 

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<PointType>::Ptr cloudOut( new pcl::PointCloud<PointType>() );
+    pcl::PointCloud<PointType>::Ptr cloudOut( new pcl::PointCloud<PointType>() );
 
     if (result == IMU){
       auto & imu = lsdk_->getIMU();

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 <ros/ros.h>
+#include <ros/package.h>
+#include <std_msgs/Header.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <std_msgs/Float32MultiArray.h>
+#include <sensor_msgs/Imu.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <sensor_msgs/NavSatFix.h>
+#include <nav_msgs/Odometry.h>
+#include <nav_msgs/Path.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+#include <tf/LinearMath/Quaternion.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_datatypes.h>
+#include <tf/transform_broadcaster.h>
+
+// PCL
+#include <pcl_conversions/pcl_conversions.h>
+
+// Eigen
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+#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<PointType>::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<sensor_msgs::PointCloud2> (cloud_topic_, 100);
+    pub_imu_ = nh.advertise<sensor_msgs::Imu> (imu_topic_, 1000);
+  }
+
+  ~UnitreeLidarSDKNodeMultiple(){
+  }
+
+  /**
+   * @brief Run once
+   */
+  bool run(){
+    MessageType result = lsdk_->runParse();
+     pcl::PointCloud<PointType>::Ptr cloudOut( new pcl::PointCloud<PointType>() );
+
+    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<PointType>::Ptr cloudOut( new pcl::PointCloud<PointType>() );
+
+    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;
+    }
+
+  }
+
+};

unitree_lidar_ros/src/unitree_lidar_ros/launch/run_multiple.launch

@@ -0,0 +1,10 @@
+<launch>
+
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find unitree_lidar_ros)/rviz/view.rviz" />
+
+  <node pkg="unitree_lidar_ros" type="unitree_lidar_ros_multiple_node"  name="unitree_lidar_ros_multiple_node" respawn="true">
+    <rosparam command="load" file="$(find unitree_lidar_ros)/config/config.yaml"/>
+
+  </node>
+
+</launch>

unitree_lidar_ros/src/unitree_lidar_ros/launch/run_without_rviz.launch

@@ -0,0 +1,10 @@
+<launch>
+
+  <!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find unitree_lidar_ros)/rviz/view.rviz" /> -->
+
+  <node pkg="unitree_lidar_ros" type="unitree_lidar_ros_node"  name="unitree_lidar_ros_node" respawn="true">
+    <rosparam command="load" file="$(find unitree_lidar_ros)/config/config.yaml"/>
+
+  </node>
+
+</launch>

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;
-}
+}

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
 )

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`
+- 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`