Update to v1.0.0

.gitignore

@@ -0,0 +1,11 @@
+install/
+
+unitree_lidar_ros/build
+unitree_lidar_ros/devel
+unitree_lidar_ros/.catkin_workspace
+unitree_lidar_ros/src/CMakeLists.txt
+
+
+unitree_lidar_ros2/build
+unitree_lidar_ros2/install
+unitree_lidar_ros2/log

README.md

@@ -1 +1,21 @@
-# unilidar_sdk
+# unilidar sdk
+
+## Introduction
+
+This repository is a SDK for [Unitree L1 LiDAR](https://www.unitree.com/LiDAR).
+
+You can use the code interfaces in this repository to obtain point cloud data and IMU data measured in our lidar, as well as configure related parameters. 
+
+We provide three commonly used interfaces for our LiDAR:
+- if you prefer to use the original **C++ SDK** directly, you can refer to [README.md](./unitree_lidar_sdk/README.md); 
+- if you want to use **ROS**, you can refer to [README.md](./unitree_lidar_ros/src/unitree_lidar_ros/README.md); 
+- if you are developing with the latest **ROS2**, you can refer to [README.md](./unitree_lidar_ros2/src/unitree_lidar_ros2/README.md).
+
+<div style="text-align:center">
+  <img src="./docs/lidar.png" width="400">
+</div>
+
+## Version History
+
+### v1.0.0 (2023.05.31)
+- Support firmware version: `1.0.1`

unitree_lidar_ros/src/unitree_lidar_ros/CMakeLists.txt

@@ -0,0 +1,60 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(unitree_lidar_ros)
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++14")
+set(CMAKE_CXX_FLAGS_DEBUG "$ENV{CXXFLAGS} -O0 -Wall -g2 -ggdb -llz4")
+set(CMAKE_CXX_FLAGS_RELEASE "$ENV{CXXFLAGS} -O3 -Wall -DNDEBUG -llz4") 
+
+find_package(PCL REQUIRED QUIET)
+
+find_package(catkin REQUIRED COMPONENTS
+  tf
+  roscpp
+  rospy
+  rosparam
+  pcl_conversions
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  nav_msgs
+  message_generation
+  visualization_msgs
+  roslib
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  DEPENDS PCL
+
+  CATKIN_DEPENDS
+  rosparam
+  std_msgs
+  nav_msgs
+  geometry_msgs
+  sensor_msgs
+  message_runtime
+  message_generation
+  visualization_msgs
+)
+
+include_directories(
+	include
+	${catkin_INCLUDE_DIRS}
+	${PCL_INCLUDE_DIRS}
+  ../../../unitree_lidar_sdk/include
+)
+
+link_directories(
+  lib
+  ${PCL_LIBRARY_DIRS}
+  ../../../unitree_lidar_sdk/lib/${CMAKE_SYSTEM_PROCESSOR}
+)
+
+set(EXTRA_LIBS 
+  ${catkin_LIBRARIES} 
+  ${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})

unitree_lidar_ros/src/unitree_lidar_ros/README.md

@@ -0,0 +1,97 @@
+# Unitree Lidar ROS
+
+## Introduction
+This package is a ROS package, which is specially used for running Unitree LiDAR products.
+
+
+The functions that this package can provide includes:
+- Parse the raw data transmitted from the lidar hardware, and convert it into pointcloud and IMU data
+- Publish the pointcloud data to a ROS topic
+- Publish the IMU data to a ROS topic
+- In addition, we provide a yaml file to configure the relevant parameters of the lidar.
+
+## Dependency
+The dependencies include `PCL` and `ROS`.
+
+We have verified that this package can successfully run under this environment:
+- `Ubuntu 20.04` 
+- `ROS noetic`
+- `PCL-1.10`
+- `unitree_lidar_sdk`
+
+You are suggested to configure an environment like this to run this package.
+
+
+## Configuration
+
+Connect your lidar to your computer with a USB cable, then confirm your serial port name for lidar:
+```
+$ ls /dev/ttyUSB*
+/dev/ttyUSB0
+```
+
+The default serial port name is `/dev/ttyUSB0`.
+If it is not the default one, you need to modify the configuration  in `unitree_lidar_ros/config/config.yaml`
+and change the `port` name to yours. For example
+```
+# Serial Port
+port: "/dev/ttyYourUSBPortName"
+```
+
+You can leave other parameters in the configuration file with their default value.
+
+If you have special needs such as changing the cloud topic name or IMU topic name, you are allowed to configure them in the configuration file as well. 
+
+The defalut cloud topic and its frame name is:
+- topic: "unilidar/cloud"
+- frame: "unilidar_lidar"
+
+The defalut IMU topic and its frame name is:
+- topic: "utlidar/imu"
+- frame: "utlidar_imu"
+
+## Build
+You can just build this ROS package as follows:
+- Clone the repository
+```
+git clone https://github.com/unitreerobotics/unilidar_sdk.git
+```
+
+- Compile
+```
+cd unilidar_sdk/unitree_lidar_ros
+
+catkin_make
+```
+
+
+## Run
+Then you need to source this ROS packege environment and then directly run the launch file:
+```
+source devel/setup.bash
+
+roslaunch unitree_lidar_ros run.launch
+```
+
+In the Rviz window, you will see our lidar pointcloud like this:
+
+![img](./docs/cloud.png)
+
+You can change the `Fixed Frame` to the imu frame `utlidar_imu`, so that you can view the IMU quaternion vector:
+
+![img](./docs/imu.png)
+
+## Version History
+
+### v1.0.0 (2022.05.04)
+- support `unitree_lidar_sdk`: v1.0.0
+
+### v1.0.1 (2022.05.05)
+- support `unitree_lidar_sdk`: v1.0.1
+
+### v1.0.2 (2022.05.12)
+- support `unitree_lidar_sdk`: v1.0.3
+
+### v1.0.3 (2022.05.30)
+- support `unitree_lidar_sdk`: v1.0.4
+

unitree_lidar_ros/src/unitree_lidar_ros/config/config.yaml

@@ -0,0 +1,22 @@
+####################################################
+# lidar sdk configuration
+####################################################
+
+# Serial Port
+port: "/dev/ttyUSB0"          # default: "/dev/ttyUSB0"
+
+# Calibration
+rotate_yaw_bias: 0.0        # default: 0 in degree
+range_scale: 0.001     # default: 0.001 in meters
+range_bias: 0                 # default: 0 in meters
+range_max: 50                 # default: 50 in meters
+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
+
+# IMU
+imu_frame: "unilidar_imu"      # default: "unilidar_imu" 
+imu_topic: "unilidar/imu"      # default: "unilidar/imu"

unitree_lidar_ros/src/unitree_lidar_ros/include/unitree_lidar_ros.h

@@ -0,0 +1,163 @@
+/**********************************************************************
+ 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 UnitreeLidarSDKNode{
+protected:
+
+  // ROS
+  ros::NodeHandle nh_;
+  ros::Publisher pub_pointcloud_raw_;
+  ros::Publisher pub_imu_;
+  tf::TransformBroadcaster tfbc1_;
+
+  // Unitree Lidar Reader
+  UnitreeLidarReader* lsdk_;
+
+  // 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_;
+
+public:
+
+  UnitreeLidarSDKNode(ros::NodeHandle nh){
+
+    // load config parameters
+    nh.param("/unitree_lidar_ros_node/port", port_, std::string("/dev/ttyUSB0"));
+
+    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();
+    lsdk_->initialize(cloud_scan_num_, port_, 2000000, rotate_yaw_bias_, 
+        range_scale_, range_bias_, range_max_, range_min_);
+
+    // ROS
+    nh_ = nh;
+    pub_pointcloud_raw_ = nh.advertise<sensor_msgs::PointCloud2> (cloud_topic_, 100);
+    pub_imu_ = nh.advertise<sensor_msgs::Imu> (imu_topic_, 1000);
+  }
+
+  ~UnitreeLidarSDKNode(){
+  }
+
+  /**
+   * @brief Run once
+   */
+  bool run(){
+    MessageType result = lsdk_->runParse();
+    static 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;
+    }
+
+  }
+};

unitree_lidar_ros/src/unitree_lidar_ros/launch/run.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>