add some open source codes which come from github

src/ros/open_source_code/ESR_canalyst_ros/.gitignore

@@ -0,0 +1,47 @@
+build/
+bin/
+lib/
+msg_gen/
+srv_gen/
+msg/*Action.msg
+msg/*ActionFeedback.msg
+msg/*ActionGoal.msg
+msg/*ActionResult.msg
+msg/*Feedback.msg
+msg/*Goal.msg
+msg/*Result.msg
+msg/_*.py
+
+# Generated by dynamic reconfigure
+*.cfgc
+/cfg/cpp/
+/cfg/*.py
+
+# Ignore generated docs
+*.dox
+*.wikidoc
+
+# eclipse stuff
+.project
+.cproject
+
+# qcreator stuff
+CMakeLists.txt.user
+
+srv/_*.py
+*.pcd
+*.pyc
+qtcreator-*
+*.user
+
+/planning/cfg
+/planning/docs
+/planning/src
+
+*~
+
+# Emacs
+.#*
+
+# Catkin custom files
+CATKIN_IGNORE

src/ros/open_source_code/ESR_canalyst_ros/.gitmodules

@@ -0,0 +1,3 @@
+[submodule "astuff_sensor_msgs"]
+	path = astuff_sensor_msgs
+	url = https://github.com/astuff/astuff_sensor_msgs.git

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/CMakeLists.txt

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(canalystii_node)
+
+find_package(catkin REQUIRED COMPONENTS
+  geometry_msgs
+  sensor_msgs
+  roscpp
+  rospy
+  std_msgs
+  canalystii_node_msg
+  tf
+  )
+
+
+include_directories(
+  include
+	${catkin_INCLUDE_DIRS} 
+)
+
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs canalystii_node_msg
+  INCLUDE_DIRS include
+)
+
+
+#add_library(canalystii src/canalystii.cpp ${catkin_LIBRARIES})
+#target_link_libraries(canalystii  ${PROJECT_SOURCE_DIR}/lib/libcontrolcan.so  ${catkin_LIBRARIES})
+
+#add_library(canalystii_node src/canalystii_node.cpp ${catkin_LIBRARIES})
+#target_link_libraries(canalystii_node ${PROJECT_SOURCE_DIR}/lib/libcontrolcan.so ${catkin_LIBRARIES})
+
+#add_executable(canalystii_node_ros src/canalystii_node_ros.cpp) 
+#target_link_libraries(canalystii_node_ros canalystii canalystii_node ${PROJECT_SOURCE_DIR}/lib/libcontrolcan.so ${catkin_LIBRARIES})
+
+add_executable(canalystii_node_ros src/canalystii_node_ros.cpp src/canalystii.cpp src/canalystii_node.cpp) 
+add_dependencies(canalystii_node_ros ${canalystii_node_msg_EXPORTED_TARGETS})
+target_link_libraries(canalystii_node_ros ${PROJECT_SOURCE_DIR}/lib/libcontrolcan.so ${catkin_LIBRARIES})

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/include/canalystii.h

@@ -0,0 +1,43 @@
+#ifndef CANALYSTII_H
+#define CANALYSTII_H
+#include "controlcan.h"
+
+// CANalyst-ii is can-usb device class, which offers
+// start_device, close_device, init_can_interface,
+// receive_can_frame, send_can_frame methods. By
+// using the provided libcontrol.so 
+// Simple usage:
+// CANalystii can_device;
+// can_device.start_device();
+// can_device.init_can_interface();
+// can_device.receive_can_frame();//listening from CAN bus
+// can_device.send_can_frame();//send to CAN bus
+class CANalystii{
+public:
+    CANalystii(int vci_device_type=4, int vci_device_ind=0);
+    ~CANalystii();
+    // start device with default parameters, and return status
+    bool start_device();
+    // close device with default parameters, and return status    
+    bool close_device();
+    // init CAN interface:port 1 or port 2
+    bool init_can_interface(unsigned int can_idx, const VCI_INIT_CONFIG& vci_conf);
+    // receive CAN frame from CAN bus
+    unsigned int receive_can_frame(unsigned int can_idx, VCI_CAN_OBJ &recv_obj, unsigned int recv_len, int wait_time=0);
+    // send CAN frame from CAN bus
+    bool send_can_frame(unsigned int can_idx, VCI_CAN_OBJ &send_obj, unsigned int send_len);
+private:
+    unsigned int vci_device_type_;
+    unsigned int vci_device_ind_;
+    
+    const unsigned int can_ind_port_[2];
+    VCI_INIT_CONFIG vci_conf_[2];
+
+    bool is_dev_start_;
+    bool is_port_init_[2];
+    bool is_port_start_[2];
+};
+
+
+
+#endif

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/include/canalystii_node.h

@@ -0,0 +1,31 @@
+#ifndef CANALYSTII_NODE_H
+#define CANALYSTII_NODE_H
+
+#include "ros/ros.h"
+#include "canalystii.h"
+#include "canalystii_node_msg/can.h"
+
+// Simple usage:
+// CANalystii_node can_device;
+// can_device.start_device();
+// can_device.init_can_interface();
+// can_device.receive_can_frame();//listening from CAN bus
+// can_device.send_can_frame();//send to CAN bus
+class CANalystii_node:public CANalystii{
+private:
+    ros::NodeHandle node_handle_;    
+    std::string can_msg_pub_topic_;
+
+    //TODO:(vincent.cheung.mcer@gmail.com) Noy yet implement subscriber.
+    //ros::Subscriber can_msg_sub_; 
+    //std::string can_msg_sub_topic_;
+    //void can_msg_cb();
+      
+public:
+    CANalystii_node(int vci_device_type=4, int vci_device_ind=0);
+    //~CANalystii_node();
+    static canalystii_node_msg::can can_obj2msg(const VCI_CAN_OBJ& can_obj);
+    static VCI_CAN_OBJ can_msg2obj(const canalystii_node_msg::can& can_msg);
+    ros::Publisher can_msg_pub_;
+};
+#endif

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/include/controlcan.h

@@ -0,0 +1,141 @@
+#ifndef CONTROLCAN1_H
+#define CONTROLCAN1_H
+
+//接口卡类型定义
+#define VCI_PCI5121		1
+#define VCI_PCI9810		2
+#define VCI_USBCAN1		3
+#define VCI_USBCAN2		4
+#define VCI_PCI9820		5
+#define VCI_CAN232		6
+#define VCI_PCI5110		7
+#define VCI_CANLite		8
+#define VCI_ISA9620		9
+#define VCI_ISA5420		10
+
+//CAN错误码
+#define	ERR_CAN_OVERFLOW			0x0001	//CAN控制器内部FIFO溢出
+#define	ERR_CAN_ERRALARM			0x0002	//CAN控制器错误报警
+#define	ERR_CAN_PASSIVE				0x0004	//CAN控制器消极错误
+#define	ERR_CAN_LOSE				0x0008	//CAN控制器仲裁丢失
+#define	ERR_CAN_BUSERR				0x0010	//CAN控制器总线错误
+
+//通用错误码
+#define	ERR_DEVICEOPENED			0x0100	//设备已经打开
+#define	ERR_DEVICEOPEN				0x0200	//打开设备错误
+#define	ERR_DEVICENOTOPEN			0x0400	//设备没有打开
+#define	ERR_BUFFEROVERFLOW			0x0800	//缓冲区溢出
+#define	ERR_DEVICENOTEXIST			0x1000	//此设备不存在
+#define	ERR_LOADKERNELDLL			0x2000	//装载动态库失败
+#define ERR_CMDFAILED				0x4000	//执行命令失败错误码
+#define	ERR_BUFFERCREATE			0x8000	//内存不足
+
+
+//函数调用返回状态值
+#define	STATUS_OK					1
+#define STATUS_ERR					0
+	
+#define USHORT unsigned short int
+#define BYTE unsigned char
+#define CHAR char
+//#define UCHAR unsigned char
+#define UINT unsigned int
+#define DWORD unsigned int
+#define PVOID void*
+#define ULONG unsigned int
+//#define INT int
+#define UINT32 UINT
+#define LPVOID void*
+#define BOOL BYTE
+#define TRUE 1
+#define FALSE 0
+
+
+#if 1
+//1.ZLGCAN系列接口卡信息的数据类型。
+typedef  struct  _VCI_BOARD_INFO{
+		USHORT	hw_Version;
+		USHORT	fw_Version;
+		USHORT	dr_Version;
+		USHORT	in_Version;
+		USHORT	irq_Num;
+		BYTE	can_Num;
+		CHAR	str_Serial_Num[20];
+		CHAR	str_hw_Type[40];
+		USHORT	Reserved[4];
+} VCI_BOARD_INFO,*PVCI_BOARD_INFO; 
+
+//2.定义CAN信息帧的数据类型。
+typedef  struct  _VCI_CAN_OBJ{
+	UINT	ID;
+	UINT	TimeStamp;
+	BYTE	TimeFlag;
+	BYTE	SendType;
+	BYTE	RemoteFlag;//是否是远程帧
+	BYTE	ExternFlag;//是否是扩展帧
+	BYTE	DataLen;
+	BYTE	Data[8];
+	BYTE	Reserved[3];
+}VCI_CAN_OBJ,*PVCI_CAN_OBJ;
+
+//3.定义CAN控制器状态的数据类型。
+typedef struct _VCI_CAN_STATUS{
+	unsigned char	ErrInterrupt;
+	unsigned char	regMode;
+	unsigned char	regStatus;
+	unsigned char	regALCapture;
+	unsigned char	regECCapture; 
+	unsigned char	regEWLimit;
+	unsigned char	regRECounter; 
+	unsigned char	regTECounter;
+	DWORD	Reserved;
+}VCI_CAN_STATUS,*PVCI_CAN_STATUS;
+
+//4.定义错误信息的数据类型。
+typedef struct _ERR_INFO{
+		UINT	ErrCode;
+		BYTE	Passive_ErrData[3];
+		BYTE	ArLost_ErrData;
+} VCI_ERR_INFO,*PVCI_ERR_INFO;
+
+//5.定义初始化CAN的数据类型
+typedef struct _INIT_CONFIG{
+	DWORD	AccCode;
+	DWORD	AccMask;
+	DWORD	Reserved;
+	unsigned char	Filter;
+	unsigned char	Timing0;	
+	unsigned char	Timing1;	
+	unsigned char	Mode;
+}VCI_INIT_CONFIG,*PVCI_INIT_CONFIG;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+DWORD VCI_OpenDevice(DWORD DeviceType,DWORD DeviceInd,DWORD Reserved);
+DWORD VCI_CloseDevice(DWORD DeviceType,DWORD DeviceInd);
+DWORD VCI_InitCAN(DWORD DeviceType, DWORD DeviceInd, DWORD CANInd, PVCI_INIT_CONFIG pInitConfig);
+
+DWORD VCI_ReadBoardInfo(DWORD DeviceType,DWORD DeviceInd,PVCI_BOARD_INFO pInfo);
+DWORD VCI_ReadErrInfo(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_ERR_INFO pErrInfo);
+DWORD VCI_ReadCANStatus(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_STATUS pCANStatus);
+
+DWORD VCI_GetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+DWORD VCI_SetReference(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,DWORD RefType,PVOID pData);
+
+ULONG VCI_GetReceiveNum(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+DWORD VCI_ClearBuffer(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+DWORD VCI_StartCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+DWORD VCI_ResetCAN(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd);
+
+ULONG VCI_Transmit(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pSend,UINT Len);
+ULONG VCI_Receive(DWORD DeviceType,DWORD DeviceInd,DWORD CANInd,PVCI_CAN_OBJ pReceive,UINT Len,int WaitTime);
+
+#ifdef __cplusplus
+}
+#endif
+#endif
+
+#endif

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/package.xml

@@ -0,0 +1,34 @@
+<?xml version="1.0"?>
+<package>
+  <name>canalystii_node</name>
+  <version>0.0.1</version>
+
+  <description>
+  canalystii_node for ros with canalystii_node message.
+  </description>
+
+  <maintainer email="vincent.cheung.mcer@gmail.com">vincent.cheung</maintainer>
+
+  <license>BSD</license>
+
+  <author email="vincent.cheung.mcer@gmail.com">vincent.cheung</author>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>roscpp</build_depend>  
+  <build_depend>rospy</build_depend>
+<build_depend>canalystii_node_msg</build_depend>
+
+  <run_depend>message_runtime</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>canalystii_node_msg</run_depend>
+  <export></export>
+
+</package>

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/src/canalystii.cpp

@@ -0,0 +1,66 @@
+#include "canalystii.h"
+
+CANalystii::CANalystii(int vci_device_type, int vci_device_ind):can_ind_port_({0,1}){
+    vci_device_type_ = vci_device_type;
+    vci_device_ind_ = vci_device_ind;
+    is_dev_start_ = false;
+    for(int i=0;i<2;i++){
+        vci_conf_[i].AccCode = 0x80000008;
+        vci_conf_[i].AccMask = 0xFFFFFFFF;
+        vci_conf_[i].Filter = 1;//receive all frames
+        vci_conf_[i].Timing0 = 0x00;
+        vci_conf_[i].Timing1 = 0x1C;//baudrate 500kbps
+        vci_conf_[i].Mode = 0;//normal mode
+        is_port_init_[i] = false;
+        is_port_start_[i] = false;
+    }
+}
+
+CANalystii::~CANalystii(){
+    if(is_dev_start_ == true){
+        close_device();
+    }
+}
+
+bool CANalystii::start_device(){
+    if(!is_dev_start_){
+        is_dev_start_ = (VCI_OpenDevice(vci_device_type_, vci_device_ind_, 0)==1);
+    }
+    return is_dev_start_;
+}
+
+bool CANalystii::close_device(){
+    if(is_dev_start_){
+        is_dev_start_ = !(VCI_CloseDevice(vci_device_type_, vci_device_ind_)==1);
+    }
+    is_port_init_[0] = false;
+    is_port_init_[1] = false;
+    is_port_start_[0] = false;
+    is_port_start_[1] = false;
+    return !is_dev_start_;
+}
+
+bool CANalystii::init_can_interface(unsigned int can_idx, const VCI_INIT_CONFIG& vci_conf){
+    unsigned int can_port_idx = can_idx<=1?can_ind_port_[can_idx]:0;
+    vci_conf_[can_port_idx] = vci_conf;
+    is_port_init_[can_port_idx] = (VCI_InitCAN(vci_device_type_, vci_device_ind_, can_port_idx,&vci_conf_[can_port_idx])==1);
+    is_port_start_[can_port_idx] = (VCI_StartCAN(vci_device_type_, vci_device_ind_, can_port_idx)==1);
+    VCI_ClearBuffer(vci_device_type_, vci_device_ind_, can_port_idx);
+    VCI_ClearBuffer(vci_device_type_, vci_device_ind_, can_port_idx);
+    return is_port_init_[can_port_idx]&&is_port_start_[can_port_idx];
+}
+
+unsigned int CANalystii::receive_can_frame(unsigned int can_idx, VCI_CAN_OBJ &recv_obj, unsigned int recv_len, int wait_time){
+    unsigned int can_port_idx = can_idx<=1?can_ind_port_[can_idx]:0;
+    unsigned int receive_len = 0;
+    receive_len = VCI_Receive(vci_device_type_, vci_device_ind_, can_port_idx, &recv_obj, recv_len, wait_time);
+    return receive_len;
+}
+
+bool CANalystii::send_can_frame(unsigned int can_idx, VCI_CAN_OBJ &send_obj, unsigned int send_len){
+    //TODO: (vincent.cheung.mcer@gmail.com) Not yet implemented.
+    unsigned int can_port_idx = can_idx<=1?can_ind_port_[can_idx]:0;    
+    bool status;
+    status = (VCI_Transmit(vci_device_type_, vci_device_ind_, can_port_idx, &send_obj, send_len)==1);
+    return status;
+}

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/src/canalystii_node.cpp

@@ -0,0 +1,52 @@
+#include "canalystii_node.h"
+
+CANalystii_node::CANalystii_node(int vci_device_type, int vci_device_ind) : 
+CANalystii(vci_device_type,vci_device_ind), node_handle_("~")
+{
+    ROS_INFO("Inititalizing CANalyst-ii node...");
+
+    node_handle_.param<std::string>("can_msg_pub_topic", can_msg_pub_topic_, "/canalyst_can");
+    ROS_INFO("can_msg_pub_topic_: %s", can_msg_pub_topic_.c_str());
+    can_msg_pub_ = node_handle_.advertise<canalystii_node_msg::can>(can_msg_pub_topic_, 10);
+
+    //TODO:(vincent.cheung.mcer@gmail.com) Noy yet implement subscriber.
+    //node_handle_.param<std::string>("can_msg_sub_topic", can_msg_sub_topic_, "/not_used_can");
+    //ROS_INFO("can_msg_sub_topic_: %s", can_msg_sub_topic_.c_str());
+    //can_msg_sub_ = node_handle_.subscribe(can_msg_sub_topic_, 2, &CANalystii_node::can_msg_cb, this);
+}
+
+canalystii_node_msg::can CANalystii_node::can_obj2msg(const VCI_CAN_OBJ& can_obj){
+    canalystii_node_msg::can can_msg;
+    //set Header with ros time rather than can_obj time, in case there are no timestamp
+    //TODO: (vincent.cheung.mcer@gmail.com) miss seq here
+    can_msg.header.frame_id = "/canalystii";
+    can_msg.header.stamp = ros::Time::now();
+
+    //set data    
+    can_msg.id = can_obj.ID;
+    can_msg.timeflag = can_obj.TimeFlag;
+    can_msg.sendtype = can_obj.SendType;
+    can_msg.remoteflag = can_obj.RemoteFlag;
+    can_msg.externflag = can_obj.ExternFlag;
+    can_msg.datalen = can_obj.DataLen;
+    for(int i =0;i<8;i++){
+        can_msg.data[i] = can_obj.Data[i];
+    }
+    return can_msg;
+}
+
+VCI_CAN_OBJ CANalystii_node::can_msg2obj(const canalystii_node_msg::can& can_msg){
+    VCI_CAN_OBJ can_obj;
+    //TODO: (vincent.cheung.mcer@gmail.com) type not consistent
+    //can_obj.TimeStamp = can_msg.header.stamp
+    can_obj.ID = can_msg.id;
+    can_obj.TimeFlag = can_msg.timeflag;
+    can_obj.SendType = can_msg.sendtype;
+    can_obj.RemoteFlag = can_msg.remoteflag;
+    can_obj.ExternFlag = can_msg.externflag;
+    can_obj.DataLen = can_msg.datalen;
+    for(int i =0;i<8;i++){
+        can_obj.Data[i] = can_msg.data[i];
+    }
+    return can_obj;    
+}

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node/src/canalystii_node_ros.cpp

@@ -0,0 +1,41 @@
+#include "ros/ros.h"
+#include "canalystii_node.h"
+
+int main(int argc, char **argv){
+    ros::init(argc, argv, "canalystii_node");
+    CANalystii_node can_node;
+    
+    if(!can_node.start_device()){
+        ROS_WARN("device starts error");
+        return -1;
+    }
+
+    VCI_INIT_CONFIG vci_conf;
+    vci_conf.AccCode = 0x80000008;
+    vci_conf.AccMask = 0xFFFFFFFF;
+    vci_conf.Filter = 1;//receive all frames
+    vci_conf.Timing0 = 0x00;
+    vci_conf.Timing1 = 0x1C;//baudrate 500kbps
+    vci_conf.Mode = 0;//normal mode
+    unsigned int can_idx = 0;
+    if(!can_node.init_can_interface(can_idx,vci_conf)){
+        ROS_WARN("device port init error");
+        return -1;
+    }
+
+    ROS_INFO("listening to can bus");
+    VCI_CAN_OBJ can_obj;
+    while(ros::ok()){
+        unsigned int recv_len = 1;
+        
+        //int len = can_node.receive_can_frame(can_idx,can_obj,recv_len,0);
+        if(can_node.receive_can_frame(can_idx,can_obj,recv_len,20)){
+            //ROS_INFO("received:%u",can_obj.ID);
+            canalystii_node_msg::can msg = CANalystii_node::can_obj2msg(can_obj);
+            can_node.can_msg_pub_.publish(msg);
+        }
+        //ros::spinOnce();
+    }
+    //ros::spin();
+    return 0;
+}

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node_msg/CMakeLists.txt

@@ -0,0 +1,35 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(canalystii_node_msg)
+
+# search for everything we need to build the messages, dont forget the message_generation
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  geometry_msgs
+  sensor_msgs
+)
+
+# search for all msg files
+FILE(GLOB messages_to_build RELATIVE "${CMAKE_CURRENT_SOURCE_DIR}/msg"
+    "${CMAKE_CURRENT_SOURCE_DIR}/msg/*.msg")
+
+# notify catkin to look at the previously found msg files
+add_message_files(
+  FILES
+  ${messages_to_build}
+)
+
+# build the header files from the msg files, and notify catkin about the dependencies
+generate_messages(
+  DEPENDENCIES
+  geometry_msgs
+  sensor_msgs
+)
+
+# export the dependencis of this package for who ever depends on us
+catkin_package(
+  CATKIN_DEPENDS message_runtime geometry_msgs
+)
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node_msg/msg/can.msg

@@ -0,0 +1,8 @@
+Header header
+uint32 id
+uint32 timeflag
+uint8 sendtype
+uint8 remoteflag
+uint8 externflag
+uint8 datalen
+uint8[8] data

src/ros/open_source_code/ESR_canalyst_ros/canalyst/canalystii_node_msg/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package>
+  <name>canalystii_node_msg</name>
+  <version>0.0.1</version>
+
+  <description>
+  canalystii_node message
+  </description>
+
+  <maintainer email="vincent.cheung.mcer@gmail.com">vincent.cheung</maintainer>
+
+  <license>BSD</license>
+
+  <author email="vincent.cheung.mcer@gmail.com">vincent.cheung</author>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+
+  <export></export>
+
+</package>

src/ros/open_source_code/ESR_canalyst_ros/canalyst/readme.md

@@ -0,0 +1,2 @@
+# CANalyst Package
+It contains the `CAN` ROS message that adapts to the one used in `CANalyst-ii`, and wrapped ROS node for further using. The ROS reads CAN frame from CAN bus and broadcast `canalystii_node_msg/can` msg with specific topic.

src/ros/open_source_code/ESR_canalyst_ros/esr_node/CMakeLists.txt

@@ -0,0 +1,113 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(esr_node)
+SET(CMAKE_CXX_FLAGS "-std=c++11 -O2 -g -Wall ${CMAKE_CXX_FLAGS}")
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  canalystii_node_msg
+  roscpp
+  rospy
+  std_msgs
+  delphi_esr_msgs
+  geometry_msgs
+  sensor_msgs
+  visualization_msgs
+)
+
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+  CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs canalystii_node_msg delphi_esr_msgs sensor_msgs visualization_msgs
+  #INCLUDE_DIRS include
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(esr_node
+#   src/${PROJECT_NAME}/esr_node.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(esr_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+add_executable(esr_node src/esr_node.cpp)
+add_dependencies(esr_node ${canalystii_node_msg_EXPORTED_TARGETS})
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(esr_node_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(esr_node
+   ${catkin_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS esr_node esr_node_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_esr_node.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/ros/open_source_code/ESR_canalyst_ros/esr_node/package.xml

@@ -0,0 +1,72 @@
+<?xml version="1.0"?>
+<package>
+  <name>esr_node</name>
+  <version>0.0.0</version>
+  <description>The esr_node package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="es@todo.todo">es</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/esr_node</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>canalystii_node_msg</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>delphi_esr_msgs</build_depend>
+  <build_depend>visualization_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+
+
+  <run_depend>canalystii_node_msg</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>rospy</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>delphi_esr_msgs</run_depend>
+  <run_depend>visualization_msgs</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/open_source_code/ESR_canalyst_ros/esr_node/readme.md

@@ -0,0 +1,2 @@
+# ESR Node
+It's the ROS Node that subscribes `canalystii_node_msg/can` and parses it into ESR_TRACK msg. Visualization marker is also broadcast, whose pose is calculated from the ESR_TRACK msg, not yet check the correctness though.

src/ros/open_source_code/ESR_canalyst_ros/esr_node/src/esr_node.cpp

@@ -0,0 +1,201 @@
+/*
+ * cluster.cpp
+ *
+ * Created on   : Oct 21, 2017
+ * Author   : Vincent Cheung
+ * @brief Below algorithm is based on Euclidean Clustering provided in PCL.
+ */
+#include <cmath>
+#include <string>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <visualization_msgs/Marker.h>
+#include <visualization_msgs/MarkerArray.h>
+
+#include "canalystii_node_msg/can.h"
+#include "delphi_esr_msgs/EsrTrack.h"
+// #include <pcl_ros/point_cloud.h>
+// #include <pcl_conversions/pcl_conversions.h>
+// #include <pcl/point_types.h>
+// #include <velodyne_pointcloud/point_types.h>
+// #include <pcl/kdtree/kdtree.h>
+// #include <pcl/kdtree/kdtree.h>
+// #include <pcl/segmentation/extract_clusters.h>
+// #include <pcl/common/common.h>
+// #include <pcl/common/centroid.h>
+
+// bytes to sign
+#define B2SIGN(x) ((x)==1?-1:1)
+// bytes to complement
+#define B2COMP1(x) ((64)-(x))
+#define B2COMP2(x) ((1024)-(x))
+#define B2COMP3(x) ((16384)-(x))
+
+class Esr_node
+{
+public:
+    
+    Esr_node();
+
+private:
+
+    ros::NodeHandle node_handle_;
+    ros::Subscriber can_node_sub_;
+    ros::Publisher esr_track_pub_;
+    ros::Publisher esr_marker_array_pub_;
+    
+    std::string can_topic_;
+    std::string esr_track_topic_;    
+    std::string marker_topic_;
+    std::string esr_frame_id_;
+
+    double marker_height_;
+    double marker_length_;
+    double marker_width_;
+    double m_pi_;
+
+    void CAN_callback(const canalystii_node_msg::can::ConstPtr &in_can_msg);
+    delphi_esr_msgs::EsrTrack set_track_msg(const canalystii_node_msg::can::ConstPtr &in_can_msg);
+    visualization_msgs::Marker track_msg2marker(const delphi_esr_msgs::EsrTrack & track_msg);
+};
+
+Esr_node::Esr_node() : node_handle_("esr_n"),m_pi_(3.1415926)
+{
+    ROS_INFO("Inititalizing ESR Rader Node...");
+
+    node_handle_.param<std::string>("can_topic_", can_topic_, "/canalyst_can");
+    ROS_INFO("can_topic_: %s", can_topic_.c_str());
+
+    node_handle_.param<std::string>("esr_track_topic_", esr_track_topic_, "/esr_track");
+    ROS_INFO("esr_track_topic_: %s", esr_track_topic_.c_str());
+
+    node_handle_.param<std::string>("marker_topic_", marker_topic_, "/esr_track_marker");
+    ROS_INFO("marker_topic_: %s", marker_topic_.c_str());
+
+    node_handle_.param<std::string>("esr_frame_id_", esr_frame_id_, "/esr_node");
+    ROS_INFO("esr_frame_id_: %s", esr_frame_id_.c_str());
+
+    node_handle_.param<double>("marker_height_", marker_height_, 0.5);
+    ROS_INFO("marker_height_: %lf", marker_height_);
+    node_handle_.param<double>("marker_length_", marker_length_, 5.0);
+    ROS_INFO("marker_length_: %lf", marker_length_);
+    node_handle_.param<double>("marker_width_", marker_width_, 2.0);
+    ROS_INFO("marker_width_: %lf", marker_width_);
+
+    can_node_sub_ = node_handle_.subscribe(can_topic_, 2, &Esr_node::CAN_callback, this);
+    esr_track_pub_ = node_handle_.advertise<delphi_esr_msgs::EsrTrack>(esr_track_topic_, 10);
+    esr_marker_array_pub_ = node_handle_.advertise<visualization_msgs::Marker>(marker_topic_, 10);
+}
+
+//TODO: (vincent.cheung.mcer@gmail.com) need to add comments to these magic bits operation
+delphi_esr_msgs::EsrTrack Esr_node::set_track_msg(const canalystii_node_msg::can::ConstPtr &in_can_msg){
+    delphi_esr_msgs::EsrTrack track_msg;
+    //set header
+    track_msg.header.frame_id = esr_frame_id_;
+    track_msg.header.stamp = in_can_msg->header.stamp; 
+    
+    track_msg.track_ID = in_can_msg->id - 0x500;//from [0x500, 0x53F] to [0x0,0x3F]
+    //is_on_coming_ = (in_can_msg->data[0]&0x01);//not inside track_msg
+    track_msg.track_group_changed = (in_can_msg->data[0] & 0x02)>>1;
+
+    //sign value
+    if(B2SIGN((in_can_msg->data[0]&0x80)>>7)==-1){
+        track_msg.track_lat_rate = (-B2COMP1((in_can_msg->data[0]&0xFC)>>2))*0.25;
+    }else{
+        track_msg.track_lat_rate = ((in_can_msg->data[0]&0xFC)>>2)*0.25;
+    }
+    
+    track_msg.track_status = (in_can_msg->data[1]&0xE0)>>5;
+
+    //sign value
+    if(B2SIGN((in_can_msg->data[1]&0x10)>>4)==-1){
+        track_msg.track_angle = -B2COMP2((((in_can_msg->data[1]&0x1F)<<5)|((in_can_msg->data[2]&0xF8)>>3)))*0.1;
+    }else{
+        track_msg.track_angle = (((in_can_msg->data[1]&0x1F)<<5)|((in_can_msg->data[2]&0xF8)>>3))*0.1;
+    }
+
+    track_msg.track_range = (((in_can_msg->data[2]&0x07)<<8)|in_can_msg->data[3])*0.1;
+    track_msg.track_bridge_object = (in_can_msg->data[4]&0x80)>>7;
+    track_msg.track_rolling_count = (in_can_msg->data[4]&0x40)>>6;
+    track_msg.track_width = ((in_can_msg->data[4]&0x3C)>>2)*0.5;
+
+    //sign value
+    if(B2SIGN((in_can_msg->data[4]&0b00000010)>>1)==-1){
+        track_msg.track_range_accel = -B2COMP2((((in_can_msg->data[4]&0x03)<<8)+in_can_msg->data[5]))*0.05;
+    }else{
+        track_msg.track_range_accel = (((in_can_msg->data[4]&0x03)<<8)+in_can_msg->data[5])*0.05;
+    }    
+
+    track_msg.track_med_range_mode = ((in_can_msg->data[6]&0xC0)>>6);
+
+    //sign value
+    if(B2SIGN((in_can_msg->data[6]&0b00100000)>>5)==-1){
+        track_msg.track_range_rate = -B2COMP3((((in_can_msg->data[6]&0x3F)<<8)|in_can_msg->data[7]))*0.01;
+    }else{
+        track_msg.track_range_rate = (((in_can_msg->data[6]&0x3F)<<8)|in_can_msg->data[7])*0.01;//sign
+    }
+
+    return track_msg;
+}
+
+
+visualization_msgs::Marker Esr_node::track_msg2marker(const delphi_esr_msgs::EsrTrack & track_msg){
+    uint32_t shape = visualization_msgs::Marker::CUBE; 
+    visualization_msgs::Marker marker; 
+    marker.header = track_msg.header;
+    
+    marker.ns = std::to_string(track_msg.track_ID); 
+    marker.id = track_msg.track_ID; 
+    marker.type = shape; 
+    marker.action = visualization_msgs::Marker::ADD; 
+    const double angle_rad = track_msg.track_angle * m_pi_/ 180;
+    marker.pose.position.x = track_msg.track_range*std::sin(angle_rad); 
+    marker.pose.position.y = track_msg.track_range*std::cos(angle_rad); 
+    marker.pose.position.z = marker_height_; 
+    marker.pose.orientation.x = 0.0; 
+    marker.pose.orientation.y = 0.0; 
+    marker.pose.orientation.z = 0.0; 
+    marker.pose.orientation.w = 1.0; 
+    
+    marker.scale.x = marker_length_; 
+    marker.scale.y = marker_width_; 
+    marker.scale.z = marker_height_; 
+          
+    marker.color.r = 10*track_msg.track_ID % 255; 
+    marker.color.g = 20*track_msg.track_ID % 255; 
+    marker.color.b = 40*track_msg.track_ID % 255; 
+    marker.color.a = 0.5; 
+  
+    marker.lifetime = ros::Duration(0.5); 
+    return marker; 
+
+}
+void Esr_node::CAN_callback(const canalystii_node_msg::can::ConstPtr &in_can_msg)
+{
+    ROS_INFO("Listening CAN frame id:%d",in_can_msg->id);
+    //check if it is not a esr track msg
+    if(in_can_msg->id<0x500 || in_can_msg->id>0x53F){
+        //TODO: (vincent.cheung.mcer@gmail.com) currently do nothing, for track_msg.id = [0x500,0x53f]
+        return ;
+    }
+
+    delphi_esr_msgs::EsrTrack track_msg = set_track_msg(in_can_msg);
+
+    visualization_msgs::Marker ma = track_msg2marker(track_msg); 
+
+    esr_marker_array_pub_.publish(ma);
+    esr_track_pub_.publish (track_msg);
+
+}
+
+int main(int argc, char **argv)
+{
+
+    ros::init(argc, argv, "Esr_radar_node");
+    Esr_node node;
+    ROS_INFO("Listening CAN frame");
+    ros::spin();
+
+    return 0;
+
+}

src/ros/open_source_code/ESR_canalyst_ros/launch/esr.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node pkg="tf"            type="static_transform_publisher" name="link2_broadcaster" args="0 0.1 0.1 -1.57 0 0 velodyne esr_node 100" />
+  <node pkg="canalystii_node" type="canalystii_node_ros" name="canalystii_node_ros"/>
+  <node pkg="esr_node" type="esr_node" name="esr_node"/>
+</launch>
+
+

src/ros/open_source_code/ESR_canalyst_ros/readme.md

@@ -0,0 +1,13 @@
+# ESR CANalyst-ii ROS package
+It contains the [CANalyst-ii](https://detail.tmall.com/item.htm?spm=a230r.1.14.6.1d07e0efi7clwK&id=542301692415&cm_id=140105335569ed55e27b&abbucket=3) driver and wrapped interface for ROS, A.W.A the ESR Radar with visualization(marker) support in Rviz. The usage and comments are updating, will soon be finished.
+
+### Screenshot
+Detected objects will be display as `Marker` in Rviz, currently the size of object is fixed now. On top of the cube marker, there is the `object ID=[0,63]`, and `acceleration speed` parsed from the ESR radar.	
+![png](./docs/Screenshot.png)
+
+### Usage
+```shell
+roslaunch esr.launch
+```
+	
+Note that there will be permission issue on the USB-Canalyst II device. If so, you have to check the USB device ID, modify the udev-usb list, and change the user group of that USB device.

src/ros/open_source_code/MobilEye630-ros-driver/README.md

@@ -0,0 +1,16 @@
+# MobilEye630-ros-driver
+
+This is Mobileye 660/630 Ros Driver 
+
+for Ros1 (catkin packaging) but it also work with ros2 if you change ropsy related function to rclpy
+
+It use socket_can bridge (its ros package) for getting CAN Message from mobileye
+
+# How to Start
+
+` roslaunch mobileye_node molbieye_node.launch`
+
+
+
+### Copyrights reserved 2021 ARTIV
+DGIST-ARTIV (Undergraduate Self Driving Research Group)

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/CMakeLists.txt

@@ -0,0 +1,219 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mobileye_msgs)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  message_generation
+  std_msgs
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+add_message_files(DIRECTORY msg
+   FILES
+   LKAlane.msg
+   ReferencePoints.msg
+   LKA.msg
+   Lane.msg
+   ObstacleStatus.msg
+   ObstacleData.msg
+   CarInfo.msg
+   Gyro.msg
+   SystemWarnings.msg
+   TSR.msg
+   TSRVisionOnlySign.msg
+   ExtLogData.msg
+   SmartADAS.msg
+   SeeQ.msg
+   StandardCAN.msg
+   MobileyeInfo.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES mobileye_msgs
+  CATKIN_DEPENDS message_runtime std_msgs
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/mobileye_msgs.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/mobileye_msgs_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_mobileye_msgs.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/CarInfo.msg

@@ -0,0 +1,15 @@
+bool high_beam
+bool low_beam
+bool wipers
+bool right_signal
+bool left_signal
+bool brake_signal
+bool wipers_available
+bool low_beam_available
+bool high_beam_available
+bool right_blink_available
+bool left_blink_available
+bool brake_available
+bool speed_available
+uint8 speed
+uint8 shield_plus_settings

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/ExtLogData.msg

@@ -0,0 +1,6 @@
+Header header
+
+Lane lane
+
+ObstacleStatus obstacle_status
+ObstacleData[] obstacle_data

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/Gyro.msg

@@ -0,0 +1,2 @@
+bool gyro_sensor_data_available
+float32 x_axis_data

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/LKA.msg

@@ -0,0 +1,10 @@
+Header header
+
+LKAlane left_lane
+LKAlane right_lane
+
+ReferencePoints reference_points
+
+uint8 number_of_next_lane_markers
+
+LKAlane[] next_lane

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/LKAlane.msg

@@ -0,0 +1,12 @@
+uint8 lane_type
+uint8 quality
+uint8 model_degree
+
+float32 position_parameter_c0
+float32 curvature_parameter_c2
+float32 curvature_derivative_c3
+float32 width_marking
+
+float32 heading_angle_parameter_c1
+float32 view_range
+bool view_range_availability

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/Lane.msg

@@ -0,0 +1,9 @@
+float32 lane_curvature
+float32 lane_heading
+
+bool ca
+float32 pitch_angle
+float32 yaw_angle
+
+bool right_ldw_availability
+bool left_ldw_availability

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/MobileyeInfo.msg

@@ -0,0 +1,23 @@
+Header header
+
+LKAlane left_lane
+LKAlane right_lane
+
+ReferencePoints reference_points
+
+uint8 number_of_next_lane_markers
+
+LKAlane[] next_lane
+
+Lane lane
+
+ObstacleStatus obstacle_status
+ObstacleData[] obstacle_data
+
+SystemWarnings system_warnings
+CarInfo car_info
+TSR[] tsr
+TSRVisionOnlySign tsr_vision_only
+Gyro gyro
+SmartADAS smart_adas
+SeeQ seeq

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/ObstacleData.msg

@@ -0,0 +1,28 @@
+uint8 obstacle_id
+float32 obstacle_position_x
+float32 obstacle_position_y
+float32 obstacle_relative_velocity_x
+
+uint8 obstacle_type
+uint8 obstacle_status
+bool obstacle_brake_lights
+uint8 cut_in_and_out
+uint8 blinker_info
+uint8 obstacle_valid
+
+float32 obstacle_length
+float32 obstacle_width
+uint8 obstacle_age
+uint8 obstacle_lane
+uint8 cipv_flag
+
+float32 radar_position_x
+float32 radar_velocity_x
+uint8 radar_match_confidence
+uint8 matched_radar_id
+
+float32 obstacle_angle_rate
+float32 obstacle_scale_change
+float32 obstacle_object_accel_x
+bool obstacle_replaced
+float32 obstacle_angle

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/ObstacleStatus.msg

@@ -0,0 +1,9 @@
+uint8 number_of_obstacles
+uint8 timestamp
+
+bool left_close_rang_cut_in
+bool right_close_rang_cut_in
+
+uint8 go
+bool close_car
+uint8 failsafe

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/ReferencePoints.msg

@@ -0,0 +1,6 @@
+float32 ref_point1_position
+float32 ref_point1_distance
+bool ref_point1_validity
+float32 ref_point2_position
+float32 ref_point2_distance
+bool ref_point2_validity

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/SeeQ.msg

@@ -0,0 +1,9 @@
+uint32 serial_number
+uint64 production_date
+
+uint8 brain_version_major
+uint8 brain_version_minor
+uint8 mest_version_major
+uint8 mest_version_minor
+uint8 mest_version_subminor
+uint8 mest_version_patch_number

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/SmartADAS.msg

@@ -0,0 +1,49 @@
+uint8 persistent_on_off
+uint8 volume_level
+uint8 hmw_level
+uint8 ldw_volume_level
+uint8 hmw_volume_level
+uint8 ldw_level
+uint8 pedestrian_warning_level
+uint8 sli_warning_level
+uint8 blinker_reminder_level
+uint8 virtual_bumper_level
+uint8 hw_repeatable_level
+
+uint8 buzzer_min_volume
+uint8 buzzer_max_volume
+uint8 buzzer_hmw_min_volume
+uint8 buzzer_hmw_max_volume
+uint8 buzzer_ldw_min_volume
+uint8 buzzer_ldw_max_volume
+uint8 ewiii_speed_indication
+uint8 disable_system_off
+uint8 calibration_source
+uint8 ldw_min_value
+uint8 ldw_max_value
+uint8 ldw_speed
+uint8 ped_min_value
+uint8 ped_max_value
+uint8 speed_for_high_low_beam_control
+uint8 virtual_bumper_min_value
+uint8 virtual_bumper_max_value
+uint8 blinker_reminder_min_value
+uint8 blinker_reminder_max_value
+
+uint8 hmw_min_value
+uint8 hmw_max_value
+uint8 hmw_repeatable_min_value
+uint8 hmw_repeatable_max_value
+uint8 sli_min_value
+uint8 sli_max_value
+uint8 sli_delta_round_step
+uint8 sli_delta_round_upwards
+uint8 country_code
+uint8 sli_unit_speed
+uint8 tamper_alert_on_frames
+uint8 tamper_alert_off_frames
+uint8 tamper_alert_enable_j1939
+
+bool[16] menu_ticks
+
+bool[16] advanced_menu_ticks

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/StandardCAN.msg

@@ -0,0 +1,7 @@
+SystemWarnings system_warnings
+CarInfo car_info
+TSR[] tsr
+TSRVisionOnlySign tsr_vision_only
+Gyro gyro
+SmartADAS smart_adas
+SeeQ seeq

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/SystemWarnings.msg

@@ -0,0 +1,23 @@
+uint8 sound_type
+bool peds_in_dz
+bool peds_fcw
+uint8 time_indicator
+
+bool error_valid
+uint8 error_code
+bool zero_speed
+bool headway_valid
+float32 headway_measurement
+
+bool ldw_off
+bool left_ldw_on
+bool right_ldw_on
+bool fcw_on
+bool maintenance
+bool failsafe
+
+bool tsr_enabled
+bool hw_repeatable_enabled
+uint8 headway_warning_level
+uint8 tsr_warning_level
+bool tamper_alert

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/TSR.msg

@@ -0,0 +1,6 @@
+uint8 vision_only_sign_type
+uint8 supplementary_sign_type
+float32 sign_position_x
+float32 sign_position_y
+float32 sign_position_z
+uint8 filter_type

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/msg/TSRVisionOnlySign.msg

@@ -0,0 +1,9 @@
+uint8 vision_only_sign_type_display_1
+uint8 vision_only_sign_type_display_2
+uint8 vision_only_sign_type_display_3
+uint8 vision_only_sign_type_display_4
+
+uint8 supplementary_sign_type_display_1
+uint8 supplementary_sign_type_display_2
+uint8 supplementary_sign_type_display_3
+uint8 supplementary_sign_type_display_4

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_msgs/package.xml

@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mobileye_msgs</name>
+  <version>0.0.0</version>
+  <description>The mobileye_msgs package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sonjunseong@todo.todo">sonjunseong</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/mobileye_msgs</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <exec_depend>message_runtime</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_node/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(mobileye_node)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  message_generation
+)
+
+catkin_install_python(PROGRAMS
+  scripts/mobileye_node.py
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mobileye_node
+)
+
+install(FILES
+  scripts/mobileye_node.launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mobileye_node
+)
+
+generate_messages(
+  DEPENDENCIES
+  std_msgs
+)
+
+
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_node/package.xml

@@ -0,0 +1,69 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>mobileye_node</name>
+  <version>0.0.0</version>
+  <description>The mobileyetest package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sonjunseong@todo.todo">sonjunseong</maintainer>
+  <maintainer email="sonjunseong@todo.todo">shinkansan</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/mobileyetest</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_node/scripts/mobileye_node.launch

@@ -0,0 +1,9 @@
+<launch>
+	<arg name="debug_mode" default="False"/>
+    <!-- debug mode will pass pre-cond check-->
+
+    <node name="mobileye_node" pkg="mobileye_node" type="mobileye_node.py" output="screen" respawn="false">
+        <param name="debug_mode" value="$(arg debug_mode)"/>
+    </node>
+
+</launch>

src/ros/open_source_code/MobilEye630-ros-driver/mobileye_node/scripts/mobileye_node.py

@@ -0,0 +1,401 @@
+#!/usr/bin/env python 
+
+import rospy
+
+from can_msgs.msg import Frame
+from mobileye_msgs.msg import *
+
+class mobileyeSub():
+    def __init__(self):
+        rospy.init_node("dbw_mobileye_node")
+        self.mobPub = rospy.Publisher('/Mobileye_Info', MobileyeInfo, queue_size=20)
+        self.frame_ok = 0
+        self.mobmsg = MobileyeInfo()
+        self.obstacle_data = [ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData(),ObstacleData()]
+        self.next_lane = [LKAlane(),LKAlane(),LKAlane(),LKAlane(),LKAlane(),LKAlane(),LKAlane(),LKAlane()]
+        self.tsr = [TSR(),TSR(),TSR(),TSR(),TSR(),TSR(),TSR()]
+        self.tsr_num = 0
+
+    def data_parser(self, data):
+            
+        if data.id == 0x766:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+        
+            _data = self.frame_data[2]*256+self.frame_data[1]
+            if _data > 32767:
+                signed_data = (_data - 65536)
+            else:
+                signed_data = _data
+            self.mobmsg.left_lane.position_parameter_c0 = float(signed_data)/256
+            self.mobmsg.left_lane.curvature_parameter_c2 = float((self.frame_data[4]*256+self.frame_data[3])-0x7FFF)/1024/1000
+            self.mobmsg.left_lane.curvature_derivative_c3 = float((self.frame_data[6]*256+self.frame_data[5])-0x7FFF)/(1<<28)
+            self.mobmsg.left_lane.width_marking = self.frame_data[7]*0.01
+            self.mobmsg.left_lane.model_degree = self.frame_data[0]//64
+            self.mobmsg.left_lane.quality = (self.frame_data[0]%64)//16
+            self.mobmsg.left_lane.lane_type = self.frame_data[0]%16
+    
+        elif data.id == 0x767:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.left_lane.heading_angle_parameter_c1 = float((self.frame_data[1]*256+self.frame_data[0])-0x7FFF)/1024
+            self.mobmsg.left_lane.view_range = float((self.frame_data[3]%128)*256+self.frame_data[2])/256
+            self.mobmsg.left_lane.view_range_availability = self.frame_data[3]//128
+
+        elif data.id == 0x768:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            _data = self.frame_data[2]*256+self.frame_data[1]
+            if _data > 32767:
+                signed_data = (_data - 65536)
+            else:
+                signed_data = _data
+            self.mobmsg.right_lane.position_parameter_c0 = float(signed_data)/256
+            self.mobmsg.right_lane.curvature_parameter_c2 = float((self.frame_data[4]*256+self.frame_data[3])-0x7FFF)/1024/1000
+            self.mobmsg.right_lane.curvature_derivative_c3 = float((self.frame_data[6]*256+self.frame_data[5])-0x7FFF)/(1<<28)
+            self.mobmsg.right_lane.width_marking = self.frame_data[7]*0.01
+            self.mobmsg.right_lane.model_degree = self.frame_data[0]//64
+            self.mobmsg.right_lane.quality = (self.frame_data[0]%64)//16
+            self.mobmsg.right_lane.lane_type = self.frame_data[0]%16
+    
+        elif data.id == 0x769:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.right_lane.heading_angle_parameter_c1 = float((self.frame_data[1]*256+self.frame_data[0])-0x7FFF)/1024
+            self.mobmsg.right_lane.view_range = float((self.frame_data[3]%128)*256+self.frame_data[2])/256
+            self.mobmsg.right_lane.view_range_availability = self.frame_data[3]//128
+
+        elif data.id == 0x76a:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.reference_points.ref_point1_position = float((self.frame_data[1]*256+self.frame_data[0])-0x7FFF)/256
+            self.mobmsg.reference_points.ref_point1_distance = float((self.frame_data[3]%128)*256+self.frame_data[2])/256
+            self.mobmsg.reference_points.ref_point1_validity = self.frame_data[3]//128
+            self.mobmsg.reference_points.ref_point2_position = float((self.frame_data[5]*256+self.frame_data[4])-0x7FFF)/256
+            self.mobmsg.reference_points.ref_point2_distance = float((self.frame_data[7]%128)*256+self.frame_data[6])/256
+            self.mobmsg.reference_points.ref_point2_validity = self.frame_data[7]//128
+
+        elif data.id == 0x76b:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.number_of_next_lane_markers = self.frame_data[0]
+
+        elif data.id >= 0x76c and data.id < 0x76c + 16:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            lane_index = (data.id - 0x76c)//2
+            type_of_info = (data.id - 0x76c)%2
+            if type_of_info == 0:
+                _data = self.frame_data[2]*256+self.frame_data[1]
+                if _data > 32767:
+                    signed_data = (_data - 65536)
+                else:
+                    signed_data = _data
+                self.next_lane[lane_index].position_parameter_c0 = float(signed_data)/256
+                self.next_lane[lane_index].curvature_parameter_c2 = float((self.frame_data[4]*256+self.frame_data[3])-0x7FFF)/1024/1000
+                self.next_lane[lane_index].curvature_derivative_c3 = float((self.frame_data[6]*256+self.frame_data[5])-0x7FFF)/(1<<28)
+                self.next_lane[lane_index].width_marking = self.frame_data[7]*0.01
+                self.next_lane[lane_index].model_degree = self.frame_data[0]//64
+                self.next_lane[lane_index].quality = (self.frame_data[0]%64)//16
+                self.next_lane[lane_index].lane_type = self.frame_data[0]%16
+            else:
+                self.next_lane[lane_index].heading_angle_parameter_c1 = float((self.frame_data[1]*256+self.frame_data[0])-0x7FFF)/1024
+                self.next_lane[lane_index].view_range = float((self.frame_data[3]%128)*256+self.frame_data[2])/256
+                self.next_lane[lane_index].view_range_availability = self.frame_data[3]//128
+
+        elif data.id == 0x700:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.system_warnings.sound_type = self.frame_data[0]%8
+            self.mobmsg.system_warnings.time_indicator = (self.frame_data[0]%32)//8
+            self.mobmsg.system_warnings.zero_speed = (self.frame_data[1]%64)//32
+            self.mobmsg.system_warnings.headway_valid = self.frame_data[2]%2
+            self.mobmsg.system_warnings.error_valid = self.frame_data[3]%2
+            self.mobmsg.system_warnings.error_code = self.frame_data[3]//2
+            self.mobmsg.system_warnings.headway_measurement = (self.frame_data[2]//2)*0.1
+            self.mobmsg.system_warnings.ldw_off = self.frame_data[4]%2
+            self.mobmsg.system_warnings.left_ldw_on = (self.frame_data[4]%4)//2
+            self.mobmsg.system_warnings.right_ldw_on = (self.frame_data[4]%8)//4
+            self.mobmsg.system_warnings.fcw_on = (self.frame_data[4]%16)//8
+            self.mobmsg.system_warnings.maintenance = (self.frame_data[4]%128)//64
+            self.mobmsg.system_warnings.failsafe = self.frame_data[4]//128
+            self.mobmsg.system_warnings.peds_fcw = (self.frame_data[5]%4)//2
+            self.mobmsg.system_warnings.peds_in_dz = (self.frame_data[5]%8)//4
+            self.mobmsg.system_warnings.tamper_alert = (self.frame_data[5]%64)//32
+            self.mobmsg.system_warnings.tsr_enabled = self.frame_data[5]//128
+            self.mobmsg.system_warnings.tsr_warning_level = self.frame_data[6]%8
+            self.mobmsg.system_warnings.headway_warning_level = self.frame_data[7]%4
+            self.mobmsg.system_warnings.hw_repeatable_enabled = (self.frame_data[7]%8)//4
+
+        elif data.id == 0x760:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.car_info.high_beam = (self.frame_data[0]%64)//32
+            self.mobmsg.car_info.low_beam = (self.frame_data[0]%32)//16
+            self.mobmsg.car_info.wipers = (self.frame_data[0]%16)//8
+            self.mobmsg.car_info.right_signal = (self.frame_data[0]%8)//4
+            self.mobmsg.car_info.left_signal = (self.frame_data[0]%4)//2
+            self.mobmsg.car_info.brake_signal = self.frame_data[0]%2
+            self.mobmsg.car_info.high_beam_available = (self.frame_data[1]%64)//32
+            self.mobmsg.car_info.low_beam_available = (self.frame_data[1]%32)//16
+            self.mobmsg.car_info.wipers_available = (self.frame_data[1]%16)//8
+            self.mobmsg.car_info.right_blink_available = (self.frame_data[1]%8)//4
+            self.mobmsg.car_info.left_blink_available = (self.frame_data[1]%4)//2
+            self.mobmsg.car_info.brake_available = self.frame_data[1]%2
+            self.mobmsg.car_info.speed_available = self.frame_data[1]//128
+            self.mobmsg.car_info.speed = self.frame_data[2]
+            self.mobmsg.car_info.shield_plus_settings = self.frame_data[7]
+
+        elif data.id == 0x737:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            _data = self.frame_data[1]*256+self.frame_data[0]
+            if _data > 32767:
+                signed_data = (_data - 65536)
+            else:
+                signed_data = _data
+            self.mobmsg.lane.lane_curvature = signed_data*3.81*0.000001
+            _data = (self.frame_data[3]%16)*256+self.frame_data[2]
+            if _data > 2047:
+                signed_data = (_data - 4096)
+            else:
+                signed_data = _data
+            self.mobmsg.lane.lane_heading = signed_data*0.0005
+            self.mobmsg.lane.ca = (self.frame_data[3]%32)//16
+            self.mobmsg.lane.pitch_angle = float((self.frame_data[7]*256+self.frame_data[6])-0x7FFF)/1024/512
+            self.mobmsg.lane.yaw_angle = float((self.frame_data[5]*256+self.frame_data[4])-0x7FFF)/1024
+            self.mobmsg.lane.right_ldw_availability = (self.frame_data[3]%64)//32
+            self.mobmsg.lane.left_ldw_availability = (self.frame_data[3]%128)//64
+
+        elif data.id == 0x738:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            if self.frame_data[0] > 13:
+                obstacle_num = 13
+            else:
+                obstacle_num = self.frame_data[0]
+            self.mobmsg.obstacle_status.number_of_obstacles = obstacle_num
+            self.mobmsg.obstacle_status.timestamp = self.frame_data[1]
+            self.mobmsg.obstacle_status.left_close_rang_cut_in = (self.frame_data[2]%8)//4
+            self.mobmsg.obstacle_status.right_close_rang_cut_in = (self.frame_data[2]%16)//8
+            self.mobmsg.obstacle_status.go = self.frame_data[2]//16
+            self.mobmsg.obstacle_status.close_car = self.frame_data[5]%2
+            self.mobmsg.obstacle_status.failsafe = (self.frame_data[5]%32)//2
+
+        elif data.id >= 0x739 and data.id <= 0x73b+(self.mobmsg.obstacle_status.number_of_obstacles-1)*3:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            obstacle_idx = (data.id - 0x739)//3
+            type_of_data = (data.id - 0x739)%3
+            if type_of_data == 0:
+                self.obstacle_data[obstacle_idx].obstacle_id = self.frame_data[0]
+                self.obstacle_data[obstacle_idx].obstacle_position_x = ((self.frame_data[2]%16)*256+self.frame_data[1])*0.0625
+                _data = (self.frame_data[4]%4)*256 + self.frame_data[3]
+                if _data > 511:
+                    signed_data = (_data - 1024)
+                else:
+                    signed_data = _data
+                self.obstacle_data[obstacle_idx].obstacle_position_y = signed_data*0.0625
+                _data = (self.frame_data[6]%16)*256+self.frame_data[5]
+                if _data > 2047:
+                    signed_data = (_data - 4096)
+                else:
+                    signed_data = _data
+                self.obstacle_data[obstacle_idx].obstacle_relative_velocity_x = signed_data*0.0625
+                self.obstacle_data[obstacle_idx].obstacle_type = (self.frame_data[6]%128)//16
+                self.obstacle_data[obstacle_idx].obstacle_status = self.frame_data[7]%8
+                self.obstacle_data[obstacle_idx].obstacle_brake_lights = (self.frame_data[7]%16)//8
+                self.obstacle_data[obstacle_idx].cut_in_and_out = self.frame_data[4]//32
+                self.obstacle_data[obstacle_idx].blinker_info = (self.frame_data[4]%32)//4
+                self.obstacle_data[obstacle_idx].obstacle_valid = self.frame_data[7]//64
+            elif type_of_data == 1:
+                self.obstacle_data[obstacle_idx].obstacle_length = self.frame_data[0]*0.5
+                self.obstacle_data[obstacle_idx].obstacle_width = self.frame_data[1]*0.05
+                self.obstacle_data[obstacle_idx].obstacle_age = self.frame_data[2]
+                self.obstacle_data[obstacle_idx].obstacle_lane = self.frame_data[3]%4
+                self.obstacle_data[obstacle_idx].cipv_flag = (self.frame_data[3]%8)//4
+                self.obstacle_data[obstacle_idx].radar_position_x = (self.frame_data[4]*16+self.frame_data[3]//16)*0.0625
+                _data = (self.frame_data[6]%16)*256+self.frame_data[5]
+                if _data > 2047:
+                    signed_data = (_data - 4096)
+                else:
+                    signed_data = _data
+                self.obstacle_data[obstacle_idx].radar_velocity_x = signed_data*0.0625
+                self.obstacle_data[obstacle_idx].radar_match_confidence = (self.frame_data[6]%128)//16
+                self.obstacle_data[obstacle_idx].matched_radar_id = self.frame_data[7]%128
+            else:
+                _data = self.frame_data[1]*256+self.frame_data[0]
+                if _data > 32767:
+                    signed_data = (_data - 65536)
+                else:
+                    signed_data = _data
+                self.obstacle_data[obstacle_idx].obstacle_angle_rate = signed_data*0.01
+                _data = self.frame_data[3]*256+self.frame_data[2]
+                if _data > 32767:
+                    signed_data = (_data - 65536)
+                else:
+                    signed_data = _data
+                self.obstacle_data[obstacle_idx].obstacle_scale_change = signed_data*0.0002
+                _data = (self.frame_data[5]%4)*256+self.frame_data[4]
+                if _data > 511:
+                    signed_data = (_data - 1024)
+                else:
+                    signed_data = _data
+                self.obstacle_data[obstacle_idx].obstacle_object_accel_x = signed_data*0.03
+                self.obstacle_data[obstacle_idx].obstacle_replaced = (self.frame_data[5]%32)//16
+                _data = self.frame_data[7]*256+self.frame_data[6]
+                if _data > 32767:
+                    signed_data = (_data - 65536)
+                else:
+                    signed_data = _data
+                self.obstacle_data[obstacle_idx].obstacle_angle = signed_data*0.01
+
+        elif data.id == 0x703:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.gyro.gyro_sensor_data_available = self.frame_data[0]//128
+            _data = self.frame_data[1]*256+self.frame_data[2]
+            if _data > 32767:
+                signed_data = (_data - 65536)
+            else:
+                signed_data = _data
+            self.mobmsg.gyro.x_axis_data = signed_data*(-0.00875)
+
+        elif data.id >= 0x720 and data.id <= 0x726:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            tsr_idx = data.id - 0x720
+            self.tsr_num = tsr_idx
+            self.tsr[tsr_idx].vision_only_sign_type = self.frame_data[0]
+            self.tsr[tsr_idx].supplementary_sign_type = self.frame_data[1]
+            self.tsr[tsr_idx].sign_position_x = self.frame_data[2]*0.5
+            _data = self.frame_data[3]%128
+            if _data > 63:
+                signed_data = _data - 128
+            else:
+                signed_data = _data
+            self.tsr[tsr_idx].sign_position_y = signed_data*0.5
+            _data = self.frame_data[4]%64
+            if _data > 31:
+                signed_data = _data - 64
+            else:
+                signed_data = _data
+            self.tsr[tsr_idx].sign_position_z = signed_data*0.5
+            self.tsr[tsr_idx].filter_type = self.frame_data[5]
+
+        elif data.id == 0x727:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.tsr_vision_only.vision_only_sign_type_display_1 = self.frame_data[0]
+            self.mobmsg.tsr_vision_only.supplementary_sign_type_display_1 = self.frame_data[1]
+            self.mobmsg.tsr_vision_only.vision_only_sign_type_display_2 = self.frame_data[2]
+            self.mobmsg.tsr_vision_only.supplementary_sign_type_display_2 = self.frame_data[3]
+            self.mobmsg.tsr_vision_only.vision_only_sign_type_display_3 = self.frame_data[4]
+            self.mobmsg.tsr_vision_only.supplementary_sign_type_display_3 = self.frame_data[5]
+            self.mobmsg.tsr_vision_only.vision_only_sign_type_display_4 = self.frame_data[6]
+            self.mobmsg.tsr_vision_only.supplementary_sign_type_display_4 = self.frame_data[7]
+
+        elif data.id == 0x400:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.smart_adas.persistent_on_off = (self.frame_data[0]%32)//8
+            self.mobmsg.smart_adas.volume_level = self.frame_data[0]//32
+            self.mobmsg.smart_adas.hmw_level = self.frame_data[1]%16
+            self.mobmsg.smart_adas.ldw_volume_level = self.frame_data[2]%8
+            self.mobmsg.smart_adas.hmw_volume_level = (self.frame_data[2]%64)//8
+            self.mobmsg.smart_adas.ldw_level = self.frame_data[2]//64
+            self.mobmsg.smart_adas.pedestrian_warning_level = self.frame_data[3]%4
+            self.mobmsg.smart_adas.sli_warning_level = self.frame_data[4]%8
+            self.mobmsg.smart_adas.blinker_reminder_level = self.frame_data[5]%8
+            self.mobmsg.smart_adas.virtual_bumper_level = self.frame_data[6]%16
+            self.mobmsg.smart_adas.hw_repeatable_level = self.frame_data[7]%16
+
+        elif data.id == 0x401:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.smart_adas.buzzer_max_volume = self.frame_data[0]%8
+            self.mobmsg.smart_adas.buzzer_min_volume = (self.frame_data[0]%64)//8
+            self.mobmsg.smart_adas.ewiii_speed_indication = self.frame_data[0]//64
+            self.mobmsg.smart_adas.buzzer_hmw_max_volume = self.frame_data[1]%8
+            self.mobmsg.smart_adas.buzzer_hmw_min_volume = (self.frame_data[1]%64)//8
+            self.mobmsg.smart_adas.disable_system_off = self.frame_data[1]//64
+            self.mobmsg.smart_adas.buzzer_ldw_max_volume = self.frame_data[2]%8
+            self.mobmsg.smart_adas.buzzer_ldw_min_volume = (self.frame_data[2]%64)//8
+            self.mobmsg.smart_adas.calibration_source = self.frame_data[2]//64
+            self.mobmsg.smart_adas.ldw_max_value = self.frame_data[3]%4
+            self.mobmsg.smart_adas.ldw_min_value = (self.frame_data[3]%16)//4
+            self.mobmsg.smart_adas.ldw_speed = (self.frame_data[3]%128)//16
+            self.mobmsg.smart_adas.ped_max_value = self.frame_data[4]%4
+            self.mobmsg.smart_adas.ped_min_value = (self.frame_data[4]%16)//4
+            self.mobmsg.smart_adas.speed_for_high_low_beam_control = self.frame_data[4]//64
+            self.mobmsg.smart_adas.virtual_bumper_max_value = self.frame_data[6]%16
+            self.mobmsg.smart_adas.virtual_bumper_min_value = self.frame_data[6]//16
+            self.mobmsg.smart_adas.blinker_reminder_max_value = self.frame_data[7]%16
+            self.mobmsg.smart_adas.blinker_reminder_min_value = self.frame_data[7]//16
+
+        elif data.id == 0x402:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.smart_adas.hmw_max_value = self.frame_data[0]%16
+            self.mobmsg.smart_adas.hmw_min_value = self.frame_data[1]%16
+            self.mobmsg.smart_adas.hmw_repeatable_max_value = self.frame_data[2]%16
+            self.mobmsg.smart_adas.hmw_repeatable_min_value = self.frame_data[3]%16
+            self.mobmsg.smart_adas.sli_max_value = self.frame_data[4]%8
+            self.mobmsg.smart_adas.sli_min_value = (self.frame_data[4]%64)//8
+            self.mobmsg.smart_adas.sli_delta_round_step = self.frame_data[5]%16
+            self.mobmsg.smart_adas.sli_delta_round_upwards = (self.frame_data[5]%64)//16
+            self.mobmsg.smart_adas.sli_unit_speed = (self.frame_data[6]%16)//4
+            self.mobmsg.smart_adas.country_code = (self.frame_data[6]%64)//16
+            self.mobmsg.smart_adas.tamper_alert_on_frames = self.frame_data[7]%4
+            self.mobmsg.smart_adas.tamper_alert_off_frames = (self.frame_data[7]%16)//4
+            self.mobmsg.smart_adas.tamper_alert_enable_j1939 = (self.frame_data[7]%64)//16
+
+        elif data.id == 0x403:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            menuticks = self.frame_data[1]*256+self.frame_data[0]
+            ad_menuticks = self.frame_data[3]*256+self.frame_data[2]
+            for i in range(16):
+                self.mobmsg.smart_adas.menu_ticks[i] = menuticks%2
+                self.mobmsg.smart_adas.advanced_menu_ticks[i] = ad_menuticks%2
+                menuticks = menuticks//2
+                ad_menuticks = ad_menuticks//2
+
+        elif data.id == 0x410:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.seeq.serial_number = self.frame_data[2]*256*256+self.frame_data[1]*256+self.frame_data[0]
+
+        elif data.id == 0x411:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.seeq.production_date = self.frame_data[3]*256*256*256+self.frame_data[2]*256*256+self.frame_data[1]*256+self.frame_data[0]
+
+        elif data.id == 0x412:
+            self.frame_ok = 1
+            self.frame_data = bytearray(data.data)
+            self.mobmsg.seeq.brain_version_major = self.frame_data[0]
+            self.mobmsg.seeq.brain_version_minor = self.frame_data[1]
+            self.mobmsg.seeq.mest_version_major = self.frame_data[2]
+            self.mobmsg.seeq.mest_version_minor = self.frame_data[3]
+            self.mobmsg.seeq.mest_version_subminor = self.frame_data[4]
+            self.mobmsg.seeq.mest_version_patch_number = self.frame_data[5]
+
+        else:
+            self.frame_ok = 0
+
+    def data_pub(self, data):
+        self.data_parser(data)
+        if self.frame_ok == 1:
+            self.mobmsg.obstacle_data = self.obstacle_data[:self.mobmsg.obstacle_status.number_of_obstacles]
+            self.mobmsg.next_lane = self.next_lane[:self.mobmsg.number_of_next_lane_markers]
+            self.mobmsg.tsr = self.tsr[:self.tsr_num]
+        self.mobPub.publish(self.mobmsg)
+
+    def listener(self):
+        rospy.Subscriber("/received_messages", Frame, self.data_pub)
+        rospy.spin()
+
+if __name__=='__main__':
+    rossub = mobileyeSub()
+    rossub.listener()

src/ros/open_source_code/ros-delphi-esr-visualization/README.md

@@ -0,0 +1,44 @@
+# Visualize Delphi ESR with ROS 
+
+More full write-up [here](https://scratchrobotics.com/2020/08/17/visualize-delphi-esr-radar-with-ros-rviz-and-autonomousstuff-driver/)
+
+## Requirements
+- ROS, Rviz (tested on Melodic)
+- [AutonomousStuff Delphi ESR ROS driver](https://autonomoustuff.atlassian.net/wiki/spaces/RW/pages/17475947/Driver+Pack+Installation+or+Upgrade+Instructions)
+
+```bash
+sudo apt update && sudo apt install apt-transport-https
+sudo sh -c 'echo "deb [trusted=yes] https://s3.amazonaws.com/autonomoustuff-repo/ $(lsb_release -sc) main" > /etc/apt/sources.list.d/autonomoustuff-public.list'
+sudo apt update
+sudo apt install ros-$ROS_DISTRO-delphi-esr
+```
+- [socketcan_bridge ROS package](http://wiki.ros.org/socketcan_bridge)
+```bash
+sudo apt install ros-$ROS_DISTRO-socketcan-bridge
+```
+
+## ROS Launch 
+
+### Visual data from can0
+
+```bash
+# Launch driver and rviz
+roslaunch master.launch use_socketcan:="true"
+```
+
+### Visual data from vcan0
+
+```bash
+# Bring up vcan0 interface
+sudo ./setup_vcan0.sh
+
+# Launch drivers and rviz
+roslaunch master.launch use_socketcan:="true" socketcan_device:="vcan0"
+```
+
+Play sample data
+
+```bash
+canplayer vcan0=can0 -v -I sample/sample1.log
+```
+

src/ros/open_source_code/ros-delphi-esr-visualization/master.launch

@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="esr_frame_id" default="esr_1" />
+  <arg name="esr_upside_down" default="false" />
+  <arg name="viz_mature_tracks_only" default="false" />
+  <arg name="use_socketcan" default="false" />
+  <arg name="socketcan_device" default="can0" />
+
+  <node pkg="socketcan_bridge" type="socketcan_bridge_node" name="socketcan_bridge" if="$(arg use_socketcan)">
+    <param name="can_device" value="$(arg socketcan_device)" />
+    <remap from="received_messages" to="can_tx" />
+  </node>
+
+  <node pkg="delphi_esr" type="delphi_esr_can" name="delphi_esr_can">
+    <param name="sensor_frame_id" value="$(arg esr_frame_id)"/>
+    <param name="sensor_upside_down" value="$(arg esr_upside_down)"/>
+    <!-- removes the noise of new tracks in rviz and only vizualizes the mature tracks --> 
+    <param name="viz_mature_tracks_only" value="$(arg viz_mature_tracks_only)"/>
+  </node>
+
+  <node pkg="tf" type="static_transform_publisher" name="esr_1" 
+  args="0 0 0 0 0 0 world $(arg esr_frame_id) 100" />
+
+  <node type="rviz" name="rviz" pkg="rviz" args="$(dirname)/default.rviz" />
+</launch>

src/ros/open_source_code/ros-delphi-esr-visualization/setup_vcan.sh

@@ -0,0 +1,7 @@
+#!/bin/bash
+
+modprobe vcan
+ip link add dev vcan0 type vcan
+ip link set up vcan0
+
+echo "Finished setup vcan0"

src/ros/open_source_code/rslidar_sdk/.clang-format

@@ -0,0 +1,66 @@
+---
+BasedOnStyle:  Google
+AccessModifierOffset: -2
+ConstructorInitializerIndentWidth: 2
+AlignEscapedNewlinesLeft: false
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: None
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: false
+BreakBeforeBinaryOperators: false
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: true
+BinPackParameters: true
+ColumnLimit:    120
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+DerivePointerBinding: false
+PointerBindsToType: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+ObjCSpaceBeforeProtocolList: true
+PenaltyBreakBeforeFirstCallParameter: 19
+PenaltyBreakComment: 60
+PenaltyBreakString: 1
+PenaltyBreakFirstLessLess: 1000
+PenaltyExcessCharacter: 1000
+PenaltyReturnTypeOnItsOwnLine: 90
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: false
+Standard:        Auto
+IndentWidth:     2
+TabWidth:        2
+UseTab:          Never
+IndentFunctionDeclarationAfterType: false
+SpacesInParentheses: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterControlStatementKeyword: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+SortIncludes: false
+SpaceAfterCStyleCast: false
+
+# Configure each individual brace in BraceWrapping
+BreakBeforeBraces: Custom
+
+# Control of individual brace wrapping cases
+BraceWrapping: {
+    AfterClass: 'true'
+    AfterControlStatement: 'true'
+    AfterEnum : 'true'
+    AfterFunction : 'true'
+    AfterNamespace : 'true'
+    AfterStruct : 'true'
+    AfterUnion : 'true'
+    BeforeCatch : 'true'
+    BeforeElse : 'true'
+    IndentBraces : 'false'
+}
+...

src/ros/open_source_code/rslidar_sdk/CHANGELOG.md

@@ -0,0 +1,63 @@
+# Changelog
+
+## v1.3.0 - 2020-11-10
+
+### Added
+
+- Add multi-cast support
+- Add saved_by_rows argument
+- Add different point types( XYZI & XYZIRT)
+
+### Changed
+
+- Update driver core, please refer to CHANGELOG in rs_driver for details
+- Update some documents
+- Change angle_path argument to hiding parameter
+
+### Removed
+
+- Remove RSAUTO for lidar type
+- Remove device_ip argument
+
+
+
+## v1.2.1 - 2020-09-04
+
+### Fixed
+
+- Fix bug in driver core, please refer to changelog in rs_driver for details.
+
+
+## v1.2.0 - 2020-09-01
+
+### Added
+- Add camera software trigger (base on target angle)
+
+### Changed
+- Update driver core, please refer to changelog in rs_driver for details
+- Update the compiler version from C++11 to C++14
+
+
+## v1.1.0 - 2020-07-01
+
+### Added
+
+- Add ROS2 support
+
+### Changed
+- Replace while loop with cv.wait
+- Update the vector copy part 
+- Update the program structure
+
+### Removed
+- Remove some unused variables in message struct
+
+## v1.0.0 - 2020-06-01
+
+### Added
+
+- New program structure
+
+- Support ROS & Protobuf-UDP functions
+
+  

src/ros/open_source_code/rslidar_sdk/CMakeLists.txt

@@ -0,0 +1,164 @@
+cmake_minimum_required(VERSION 3.5)
+cmake_policy(SET CMP0048 NEW)
+project(rslidar_sdk)
+
+#=======================================
+# Compile setup (ORIGINAL, CATKIN, COLCON)
+#=======================================
+set(COMPILE_METHOD CATKIN)
+
+#=======================================
+# Custom Point Type (XYZI, XYZIRT)
+#=======================================
+set(POINT_TYPE XYZI)
+
+#========================
+# Project details / setup
+#========================
+set(PROJECT_NAME rslidar_sdk)
+add_definitions(-DPROJECT_PATH="${PROJECT_SOURCE_DIR}")
+set(CMAKE_BUILD_TYPE RELEASE)
+add_definitions(-O3)
+add_definitions(-std=c++14)
+add_compile_options(-Wall)
+
+#========================
+# Dependencies Setup
+#========================
+#ROS#
+find_package(roscpp 1.12 QUIET)
+if(roscpp_FOUND)
+  message(=============================================================)
+  message("-- ROS Found, Ros Support is turned On!")
+  message(=============================================================)
+  add_definitions(-DROS_FOUND)
+  include_directories(${roscpp_INCLUDE_DIRS})
+else(roscpp_FOUND)
+  message(=============================================================)
+  message("-- ROS Not Found, Ros Support is turned Off!")
+  message(=============================================================)
+endif(roscpp_FOUND)
+#ROS2#
+find_package(rclcpp QUIET)
+if(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON")
+  find_package(ament_cmake REQUIRED)
+  find_package(sensor_msgs REQUIRED)
+  find_package(rslidar_msg REQUIRED)
+  find_package(std_msgs REQUIRED)                      
+  set(CMAKE_CXX_STANDARD 14)
+  message(=============================================================)
+  message("-- ROS2 Found, Ros2 Support is turned On!")
+  message(=============================================================)
+  add_definitions(-DROS2_FOUND)
+  include_directories(${rclcpp_INCLUDE_DIRS})
+else(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON")
+  message(=============================================================)
+  message("-- ROS2 Not Found, Ros2 Support is turned Off!")
+  message(=============================================================)
+endif(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON")
+#Protobuf#
+find_package(Protobuf QUIET)
+find_program(PROTOC protoc)
+if(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
+  message(=============================================================)
+  message("-- Protobuf Found, Protobuf Support is turned On!")
+  message(=============================================================)
+  add_definitions(-DPROTO_FOUND)
+  include_directories(${PROTOBUF_INCLUDE_DIRS})
+  SET(PROTO_FILE_PATH ${PROJECT_SOURCE_DIR}/src/msg/proto_msg)
+  file(GLOB PROTOBUF_FILELIST ${PROTO_FILE_PATH}/*.proto)
+  foreach(proto_file ${PROTOBUF_FILELIST})
+    message(STATUS "COMPILING ${proto_file} USING ${PROTOBUF_PROTOC_EXECUTABLE}")
+    execute_process(COMMAND ${PROTOBUF_PROTOC_EXECUTABLE}
+                    --proto_path=${PROTO_FILE_PATH}
+                    --cpp_out=${PROTO_FILE_PATH}
+                    ${proto_file})
+  endforeach()
+else(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
+  message(=============================================================)
+  message("-- Protobuf Not Found, Protobuf Support is turned Off!")
+  message(=============================================================)
+endif(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
+#Others#
+find_package(yaml-cpp REQUIRED)
+find_package(PCL QUIET REQUIRED)
+include_directories(${PCL_INCLUDE_DIRS})
+link_directories(${PCL_LIBRARY_DIRS})
+add_definitions(${PCL_DEFINITIONS})
+#Catkin#
+if(${COMPILE_METHOD} STREQUAL "CATKIN")
+  find_package(catkin REQUIRED COMPONENTS
+          roscpp
+          sensor_msgs
+          )
+
+  catkin_package(
+          CATKIN_DEPENDS sensor_msgs
+  )
+endif(${COMPILE_METHOD} STREQUAL "CATKIN")
+#Include directory#
+include_directories(${PROJECT_SOURCE_DIR}/src)
+#Driver core#
+add_subdirectory(src/rs_driver)
+find_package(rs_driver REQUIRED)
+include_directories(${rs_driver_INCLUDE_DIRS})
+
+#========================
+# Point Type Definition
+#========================
+if(${POINT_TYPE} STREQUAL "XYZI")
+add_definitions(-DPOINT_TYPE_XYZI)
+elseif(${POINT_TYPE} STREQUAL "XYZIRT")
+add_definitions(-DPOINT_TYPE_XYZIRT)
+endif()
+message(=============================================================)
+message("-- POINT_TYPE is ${POINT_TYPE}")
+message(=============================================================)
+
+#========================
+# Build Setup
+#========================
+#Protobuf#
+if(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
+  add_executable(rslidar_sdk_node
+              node/rslidar_sdk_node.cpp
+              src/manager/adapter_manager.cpp
+              ${PROTO_FILE_PATH}/packet.pb.cc
+              ${PROTO_FILE_PATH}/scan.pb.cc
+              ${PROTO_FILE_PATH}/point_cloud.pb.cc
+              )
+  target_link_libraries(rslidar_sdk_node                   
+                      ${PCL_LIBRARIES}
+                      ${YAML_CPP_LIBRARIES}
+                      ${PROTOBUF_LIBRARY}
+                      ${rs_driver_LIBRARIES}
+                      )
+else(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
+  add_executable(rslidar_sdk_node
+              node/rslidar_sdk_node.cpp
+              src/manager/adapter_manager.cpp
+              )
+  target_link_libraries(rslidar_sdk_node                   
+                      ${PCL_LIBRARIES}
+                      ${YAML_CPP_LIBRARIES}
+                      ${rs_driver_LIBRARIES}
+                      )
+endif(NOT PROTOC MATCHES "NOTFOUND" AND Protobuf_FOUND)
+#Ros#
+if(roscpp_FOUND)
+  target_link_libraries(rslidar_sdk_node ${roscpp_LIBRARIES})
+endif(roscpp_FOUND)
+#Ros2#
+if(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON")
+  ament_target_dependencies(rslidar_sdk_node rclcpp sensor_msgs std_msgs rslidar_msg)
+  install(TARGETS
+    rslidar_sdk_node
+    DESTINATION lib/${PROJECT_NAME}
+  )
+  install(DIRECTORY
+    launch
+    rviz
+    DESTINATION share/${PROJECT_NAME}
+  )
+  ament_package()
+endif(rclcpp_FOUND AND ${COMPILE_METHOD} STREQUAL "COLCON")

src/ros/open_source_code/rslidar_sdk/LICENSE

@@ -0,0 +1,19 @@
+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.
+

src/ros/open_source_code/rslidar_sdk/README.md

@@ -0,0 +1,203 @@
+# **rslidar_sdk**
+
+ [中文介绍](README_CN.md) 
+
+## 1 Introduction
+
+**rslidar_sdk** is the lidar driver software development kit running on Ubuntu operating system, which contains the lidar driver core, ROS support, ROS2 support and Protobuf-UDP communication functions. For users who want to get point cloud through ROS or ROS2,  this software development kit can be used directly. For those who want to do advanced development or integrate the lidar driver into their own projects, please refer to the lidar driver core [rs_driver](https://github.com/RoboSense-LiDAR/rs_driver).
+
+### **1.1 LiDAR Support**
+
+- RS16
+- RS32
+- RSBP
+- RS128
+- RS80
+- RSM1-B3
+- RSHELIOS
+
+### 1.2 Point type support
+
+- XYZI - x, y, z, intensity
+- XYZIRT - x, y, z, intensity, ring, timestamp
+
+## 2 Download
+
+### 2.1 Download via Git 
+
+Since rslidar_sdk project includes a submodule --- rs_driver, user needs to run the following commands after git clone to download the submodule properly.
+
+```sh
+git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git
+cd rslidar_sdk
+git submodule init
+git submodule update
+```
+
+### 2.2 Download directly
+
+Instead of using Git, user can also access [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) to download the latest version of rslidar_sdk. Please download the **rslidar_sdk.tar.gz** archive instead of Source code because the Source code zip file does not contain the submodule(rs_driver), which should be downloaded manaully.
+
+![](doc/img/download_page.png)
+
+## 3 Dependencies
+
+### 3.1 ROS
+
+To run rslidar_sdk in ROS environment, ROS related libraries need to be installed. 
+
+**Ubuntu 16.04**: ros-kinetic-desktop-full
+
+**Ubuntu 18.04**: ros-melodic-desktop-full
+
+**Installation**: please refer to  http://wiki.ros.org
+
+If you install ros-kinetic-desktop-full or ros-melodic-desktop-full， the corresponding PCL and Boost  will be installed at the same time. It will bring you a lot of convenience since you don't need to handle the version confliction. Thus, **it's highly recommanded to install ros-distro-desktop-full**.
+
+### 3.2 ROS2
+
+If use rslidar_sdk in ROS2 environment, ROS2 related libraries need to be installed. 
+
+**Ubuntu 16.04**: Not supportted
+
+**Ubuntu 18.04**: ROS2 eloquent desktop
+
+**Installation**: please refer to https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/
+
+**Note! Please avoid installing ROS and ROS2 on the same computer! This may cause conflict! Also you may need to install Yaml manually.**
+
+### 3.3 Yaml(Essential) 
+
+version: >= v0.5.2
+
+*If ros-distro-desktop-full is installed, this section can be skipped*
+
+Installation:
+
+```sh
+sudo apt-get update
+sudo apt-get install -y libyaml-cpp-dev
+```
+
+### 3.4 Pcap(Essential) 
+
+version: >=v1.7.4
+
+Installation：
+
+```sh
+sudo apt-get install -y  libpcap-dev
+```
+
+### 3.5 Protobuf(Optional)
+
+version:>=v2.6.1
+
+Installation :
+
+```sh
+sudo apt-get install -y libprotobuf-dev protobuf-compiler
+```
+
+
+
+## 4 Compile & Run
+
+We offer three ways to compile and run the driver.
+
+### 4.1 Compile directly
+
+ In this way, user can use ROS facilities(not ROS2) if ROS master node is launched in advance via ```roscore```. Also, rviz can be launched seperately for visualization. 
+
+```sh
+cd rslidar_sdk
+mkdir build && cd build
+cmake .. && make -j4
+./rslidar_sdk_node
+```
+
+
+
+### 4.2 Compile with ROS catkin tools
+
+(1) Open the *CMakeLists.txt* in the project，modify the line  on top of the file **set(COMPILE_METHOD ORIGINAL)** to **set(COMPILE_METHOD CATKIN)**.
+
+```cmake
+#=======================================
+# Compile setup (ORIGINAL,CATKIN,COLCON)
+#=======================================
+set(COMPILE_METHOD CATKIN)
+```
+
+(2) Rename the file *package_ros1.xml*  in the rslidar_sdk to *package.xml*
+
+(3) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project into the *src* folder.
+
+(4) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source devel/setup.zsh*).
+
+```sh
+catkin_make
+source devel/setup.bash
+roslaunch rslidar_sdk start.launch
+```
+
+### 4.3 Compile with ROS2-colcon
+
+(1) Open the *CMakeLists.txt* in the project，modify the line  on top of the file **set(COMPILE_METHOD ORIGINAL)** to **set(COMPILE_METHOD COLCON)**.
+
+```cmake
+#=======================================
+# Compile setup (ORIGINAL,CATKIN,COLCON)
+#=======================================
+set(COMPILE_METHOD COLCON)
+```
+
+(2) Rename the file *package_ros2.xml*  in the rslidar_sdk to *package.xml*
+
+(3) Create a new workspace folder, and create a *src* folder in it. Then put the rslidar_sdk project in the *src* folder.
+
+(4) Download the packet definition project in ROS2 through [link](https://github.com/RoboSense-LiDAR/rslidar_msg), then put the project rslidar_msg in the *src* folder you just created.
+
+(5) Go back to the root of workspace, run the following commands to compile and run. (if using zsh, replace the 2nd command with *source install/setup.zsh*).
+
+```sh
+colcon build
+source install/setup.bash
+ros2 launch rslidar_sdk start.py
+```
+
+
+
+## 5 Introduction to parameters
+
+This section is very important since all functions and features provided by rslidar_sdk is configured via parameter modification. So please read the following links carefully. 
+
+[Intro to parameters](doc/intro/parameter_intro.md)
+
+[Intro to hidden parameters](doc/intro/hiding_parameters_intro.md)
+
+
+
+## 6 Quick start
+
+The following documents are some quick guides for using some of the most common features of the rslidar_sdk.  The rslidar_sdk are not limited to the following modes of operation and users can use rslidar_sdk in their own way by modifying parameters.
+
+[Online connect lidar and send point cloud through ROS](doc/howto/how_to_online_send_point_cloud_ros.md)
+
+[Record rosbag & Offline decode rosbag](doc/howto/how_to_record_and_offline_decode_rosbag.md)
+
+[Decode pcap bag and send point cloud through ROS](doc/howto/how_to_offline_decode_pcap.md)
+
+
+
+## 7 Advanced
+
+[Send & Receive via Protobuf](doc/howto/how_to_use_protobuf_function.md)
+
+[Multi-LiDARs](doc/howto/how_to_use_multi_lidars.md)
+
+[Switch Point Type](doc/howto/how_to_switch_point_type.md) 
+
+[Coordinate Transformation](doc/howto/how_to_use_coordinate_transformation.md) 
+
+[Multi-Cast](doc/howto/how_to_use_multi_cast_function.md) 

src/ros/open_source_code/rslidar_sdk/README_CN.md

@@ -0,0 +1,200 @@
+# **rslidar_sdk**
+
+## 1 工程简介
+ **rslidar_sdk** 为速腾聚创在Ubuntu环境下的雷达驱动软件包，包括了雷达驱动内核， ROS拓展功能，ROS2拓展功能，Protobuf-UDP通信拓展功能。对于没有二次开发需求的用户，或是想直接使用ROS或ROS2进行二次开发的用户，可直接使用本软件包， 配合ROS或ROS2自带的RVIZ可视化工具即可查看点云。 对于有更深一步二次开发需求，想将雷达驱动集成到自己工程内的客户， 请参考雷达驱动内核的相关文档，直接使用内核[rs_driver](https://github.com/RoboSense-LiDAR/rs_driver)进行二次开发。
+
+### **1.1 雷达型号支持**
+
+- RS16
+- RS32
+- RSBP
+- RS128
+- RS80
+- RSM1-B3
+- RSHELIOS
+
+### 1.2 点的类型支持
+
+- XYZI - x, y, z, intensity
+- XYZIRT - x, y, z, intensity, ring, timestamp
+
+## 2 下载
+
+### 2.1 使用git clone
+
+ 由于rslidar_sdk项目中包含子模块驱动内核rs_driver, 因此在执行git clone 后还需要执行相关指令初始化并更新子模块。
+
+  ```sh
+git clone https://github.com/RoboSense-LiDAR/rslidar_sdk.git
+cd rslidar_sdk
+git submodule init
+git submodule update
+  ```
+
+### 2.2 直接下载
+
+用户可以直接访问  [rslidar_sdk_release](https://github.com/RoboSense-LiDAR/rslidar_sdk/releases) 下载最新版本的rslidar_sdk. 请下载 **rslidar_sdk.tar.gz** 压缩包， 不要下载Source code。 因为Source code压缩包内不包含子模块rs_driver的代码， 用户还需自行下载rs_driver的代码放入其中才行。
+
+![](doc/img/download_page.png)
+
+
+
+## 3 依赖介绍
+
+### 3.1 ROS 
+
+*若需在ROS环境下使用雷达驱动，则需安装ROS相关依赖库*
+
+Ubuntu 16.04 - ROS kinetic desktop-full  
+
+Ubuntu 18.04 - ROS melodic desktop-full
+
+安装方式： 参考 http://wiki.ros.org
+
+**如果安装了ROS kinetic desktop-full版或ROS melodic desktop-full版，那么兼容版本其他依赖库也应该同时被安装了，所以不需要重新安装它们以避免多个版本冲突引起的问题, 因此，强烈建议安装desktop-full版，这将节省大量的时间来逐个安装和配置库**。
+
+### 3.2 ROS2
+
+*若需在ROS2环境下使用雷达驱动，则需安装ROS2相关依赖库*
+
+Ubuntu 16.04 - 不支持
+
+Ubuntu 18.04 - ROS2 Eloquent desktop
+
+安装方式：参考 https://index.ros.org/doc/ros2/Installation/Eloquent/Linux-Install-Debians/
+
+**注意！ 请避免在同一台电脑上同时安装ROS和ROS2， 这可能会产生冲突！ 同时还需要手动安装Yaml库。**
+
+### 3.3 Yaml (必需)
+
+版本号:  >= v0.5.2 
+
+*若已安装ROS desktop-full, 可跳过*
+
+安装方式:
+
+```sh
+sudo apt-get update
+sudo apt-get install -y libyaml-cpp-dev
+```
+
+### 3.4 Pcap (必需)
+
+版本号： >=v1.7.4
+
+安装方式：
+
+```sh
+sudo apt-get install -y  libpcap-dev
+```
+
+### 3.5 Protobuf (可选)
+
+版本号： >=v2.6.1
+
+安装方式:
+
+```sh
+sudo apt-get install -y libprotobuf-dev protobuf-compiler
+```
+
+
+
+## 4 编译 & 运行
+
+我们提供三种编译&运行方式。
+
+### 4.1 直接编译
+
+按照如下指令即可编译运行程序。 直接编译也可以使用ROS相关功能(不包括ROS2)，但需要在程序启动前**手动启动roscore**，启动后**手动打开rviz**才能看到可视化点云结果。
+
+```sh
+cd rslidar_sdk
+mkdir build && cd build
+cmake .. && make -j4
+./rslidar_sdk_node
+```
+
+### 4.2 依赖于ROS-catkin编译
+
+(1) 打开工程内的*CMakeLists.txt*文件，将文件顶部的**set(COMPILE_METHOD ORIGINAL)**改为**set(COMPILE_METHOD CATKIN)**。
+
+```cmake
+#=======================================
+# Compile setup (ORIGINAL,CATKIN,COLCON)
+#=======================================
+set(COMPILE_METHOD CATKIN)
+```
+
+(2) 将rslidar_sdk工程目录下的*package_ros1.xml*文件重命名为*package.xml*。
+
+(3) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。
+
+(4) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 *source devel/setup.zsh*)。
+
+```sh
+catkin_make
+source devel/setup.bash
+roslaunch rslidar_sdk start.launch
+```
+
+### 4.3 依赖于ROS2-colcon编译
+
+(1) 打开工程内的*CMakeLists.txt*文件，将文件顶部的**set(COMPILE_METHOD ORIGINAL)**改为**set(COMPILE_METHOD COLCON)**。
+
+```cmake
+#=======================================
+# Compile setup (ORIGINAL,CATKIN,COLCON)
+#=======================================
+set(COMPILE_METHOD COLCON)
+```
+
+(2) 将rslidar_sdk工程目录下的*package_ros2.xml*文件重命名为*package.xml*。
+
+(3) 新建一个文件夹作为工作空间，然后再新建一个名为*src*的文件夹, 将rslidar_sdk工程放入*src*文件夹内。
+
+(4) 通过[链接](https://github.com/RoboSense-LiDAR/rslidar_msg)下载ROS2环境下的雷达packet消息定义， 将rslidar_msg工程也放在刚刚新建的*src*文件夹内，与rslidar_sdk并列。
+
+(5) 返回工作空间目录，执行以下命令即可编译&运行(若使用.zsh,将第二句指令替换为 *source install/setup.zsh*)。
+
+```sh
+colcon build
+source install/setup.bash
+ros2 launch rslidar_sdk start.py
+```
+
+
+
+## 5 参数介绍
+
+参数介绍非常重要，请仔细阅读。 本软件包的所有功能都将通过配置参数文件来实现。
+
+[参数介绍](doc/intro/parameter_intro_cn.md)
+
+[隐藏参数介绍](doc/intro/hiding_parameters_intro_cn.md)
+
+
+
+## 6 快速上手
+
+以下仅为一些常用功能的快速使用指南， 实际使用时并不仅限于以下几种工作模式， 可通过配置参数改变不同的工作模式。
+
+[在线读取雷达数据发送到ROS](doc/howto/how_to_online_send_point_cloud_ros_cn.md)
+
+[录制ROS数据包&离线解析ROS数据包](doc/howto/how_to_record_and_offline_decode_rosbag_cn.md)
+
+[离线解析Pcap包发送到ROS](doc/howto/how_to_offline_decode_pcap_cn.md)
+
+
+
+## 7 使用进阶
+
+[使用Protobuf发送&接收](doc/howto/how_to_use_protobuf_function_cn.md)
+
+[多雷达](doc/howto/how_to_use_multi_lidars_cn.md)
+
+[切换点的类型](doc/howto/how_to_switch_point_type_cn.md) 
+
+[坐标变换功能](doc/howto/how_to_use_coordinate_transformation_cn.md) 
+
+[组播模式](doc/howto/how_to_use_multi_cast_function_cn.md) 

src/ros/open_source_code/rslidar_sdk/config/config.yaml

@@ -0,0 +1,41 @@
+common:
+  msg_source: 1                                         #0: not use Lidar
+                                                        #1: packet message comes from online Lidar
+                                                        #2: packet message comes from ROS or ROS2
+                                                        #3: packet message comes from Pcap file
+                                                        #4: packet message comes from Protobuf-UDP
+                                                        #5: point cloud comes from Protobuf-UDP
+  send_packet_ros: false                                #true: Send packets through ROS or ROS2(Used to record packet)
+  send_point_cloud_ros: true                            #true: Send point cloud through ROS or ROS2
+  send_packet_proto: false                              #true: Send packets through Protobuf-UDP
+  send_point_cloud_proto: false                         #true: Send point cloud through Protobuf-UDP
+  pcap_path: /home/nvidia/lidar.pcap                 #The path of pcap file
+
+lidar:
+  - driver:
+      lidar_type: RS16            #LiDAR type - RS16, RS32, RSBP, RS128, RS80, RSM1, RSHELIOS
+      frame_id: /rslidar           #Frame id of message
+      msop_port: 6699              #Msop port of lidar
+      difop_port: 7788             #Difop port of lidar
+      start_angle: 0               #Start angle of point cloud
+      end_angle: 360               #End angle of point cloud 
+      min_distance: 0.2            #Minimum distance of point cloud
+      max_distance: 200            #Maximum distance of point cloud
+      use_lidar_clock: false       #True--Use the lidar clock as the message timestamp
+                                   #False-- Use the system clock as the timestamp  
+    ros:
+      ros_recv_packet_topic: /rslidar_packets          #Topic used to receive lidar packets from ROS
+      ros_send_packet_topic: /rslidar_packets          #Topic used to send lidar packets through ROS
+      ros_send_point_cloud_topic: /rslidar_points      #Topic used to send point cloud through ROS
+    proto:
+      point_cloud_recv_port: 60021                     #Port number used for receiving point cloud 
+      point_cloud_send_port: 60021                     #Port number which the point cloud will be send to
+      msop_recv_port: 60022                            #Port number used for receiving lidar msop packets
+      msop_send_port: 60022                            #Port number which the msop packets will be send to 
+      difop_recv_port: 60023                           #Port number used for receiving lidar difop packets
+      difop_send_port: 60023                           #Port number which the difop packets will be send to 
+      point_cloud_send_ip: 127.0.0.1                   #Ip address which the point cloud will be send to 
+      packet_send_ip: 127.0.0.1                        #Ip address which the lidar packets will be send to
+
+
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_offline_decode_pcap.md

@@ -0,0 +1,70 @@
+# How to decode pcap bag and send point cloud to ROS
+
+## 1 Introduction
+
+This document will show you how to decode pcap bag  and send point cloud to ROS. Please make sure you have read the LiDAR user-guide and [Intro to parameters](doc/intro/parameter_intro.md) before reading this document.
+
+## 2 Steps
+
+### 2.1 Get the LiDAR port number
+
+Please follow the instructions in LiDAR user-guide to connect the LiDAR and set up your computer's ip address. At this time, you should have already known your LiDAR's msop port number and difop port number. The default is ```msop-6699, difop-7788```. If you have no idea about what it is, please check the LiDAR user-guide first.
+
+### 2.2 Set up the common part of the config file
+
+```yaml
+common:
+  msg_source: 3                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap               
+```
+
+Since the message come from the pcap bag, set ```msg_source = 3```. 
+
+Send point cloud to ROS so set ```send_point_cloud_ros = true```. 
+
+Make sure the ```pcap_path``` is correct.
+
+### 2.3 Set up the lidar-driver part of the config file
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788           
+      start_angle: 0               
+      end_angle: 360              
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false      
+```
+
+Set the ```lidar_type```  to your LiDAR type.
+
+Set the ```msop_port``` and ```difop_port```  to your LiDAR's port number. 
+
+### 2.4 Set up the lidar-ros part of the config file
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets   
+  ros_send_point_cloud_topic: /rslidar_points      
+```
+
+Set the ```ros_send_point_cloud_topic```  to the topic you want to send. 
+
+### 2.5 Run
+
+Run the program. 
+
+
+
+
+
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_offline_decode_pcap_cn.md

@@ -0,0 +1,65 @@
+# 如何解码pcap包并发送点云数据到ROS
+
+## 1 简介
+
+本文档将展示如何解码pcap包并发送点云数据到ROS。 在阅读本文档之前，请确保已阅读雷达用户手册和 [参数简介](../intro/parameter_intro.md) 。
+
+## 2 步骤
+
+### 2.1 获取数据端口号
+
+首先根据雷达用户手册连接雷达并设置好您的电脑的IP地址。此时应该已知雷达的msop端口号和difop端口号，默认值为```msop-6699, difop-7788```。 如果不清楚上述内容，请查看雷达用户手册。
+
+### 2.2 设置参数文件的common部分
+
+```yaml
+common:
+  msg_source: 3                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap        
+```
+
+由于消息来自pcap包，因此请设置 ```msg_source = 3``` 。
+
+将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。 
+
+请确保路径```pcap_path``` 是正确的。
+
+### 2.3 设置参数文件的 lidar-driver部分
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788           
+      start_angle: 0               
+      end_angle: 360              
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false      
+```
+
+将 ```lidar_type``` 设置为LiDAR类型 。
+
+
+设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。
+
+### 2.4设置配置文件的lidar-ros部分
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets    
+  ros_send_point_cloud_topic: /rslidar_points     
+```
+
+将 ```ros_send_point_cloud_topic``` 设置为发送点云的话题。 
+
+#### 2.5 运行
+
+运行程序。

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_online_send_point_cloud_ros.md

@@ -0,0 +1,70 @@
+# How to online connect lidar and send point cloud to ROS
+
+## 1 Introduction
+
+This document will show you how to online connect a LiDAR and send point cloud to ROS. Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/parameter_intro.md) before reading this document.
+
+## 2 Steps
+
+### 2.1 Get the LiDAR port number
+
+Please follow the instructions in LiDAR user-guide to connect the LiDAR and set up your computer's ip address. At this time, you should have already known your LiDAR's msop port number and difop port number. The default is ```msop-6699, difop-7788```. If you have no idea about what it is, please check the LiDAR user-guide first.
+
+### 2.2 Set up the common part of the config file
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+```
+
+Since the message come from the LiDAR, set ```msg_source = 1```. 
+
+Send point cloud to ROS so set ```send_point_cloud_ros = true```.
+
+### 2.3 Set up the lidar-driver part of the config file
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788           
+      start_angle: 0               
+	Run the demo & play the rosbag.
+
+      end_angle: 360              
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false    
+```
+
+Set the ```lidar_type```  to your LiDAR type.
+
+Set the ```msop_port``` and ```difop_port```  to your LiDAR's port number. 
+
+### 2.4 Set up the lidar-ros part of the config file
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets   
+  ros_send_point_cloud_topic: /rslidar_points      
+```
+
+Set the ```ros_send_point_cloud_topic```  to the topic you want to send. 
+
+### 2.5 Run
+
+Run the program. 
+
+
+
+
+
+ 

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_online_send_point_cloud_ros_cn.md

@@ -0,0 +1,65 @@
+# 如何在线连接雷达并发送点云数据到ROS
+
+## 1 简介
+
+本文档描述了如何在线连接雷达并发送点云数据到ROS。在阅读本文档之前， 请确保已经阅读过雷达用户手册和[参数简介](../intro/parameter_intro.md) 。
+
+## 2 步骤
+
+### 2.1 获取数据端口号
+
+首先根据雷达用户手册连接雷达并设置好您的电脑的IP地址。此时应该已知雷达的msop端口号和difop端口号，默认值为```msop-6699, difop-7788```。 如果不清楚上述内容，请查看雷达用户手册。
+
+### 2.2 设置参数文件的common部分
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+```
+
+由于消息来源于在线雷达，因此请设置```msg_source=1```。
+
+将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。
+
+### 2.3 设置参数文件的 lidar-driver部分
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788           
+      start_angle: 0               
+      end_angle: 360              
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false    
+```
+
+将 ```lidar_type``` 设置为LiDAR类型 。
+
+
+设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。
+
+### 2.4设置配置文件的lidar-ros部分
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets    
+  ros_send_point_cloud_topic: /rslidar_points     
+```
+
+将 ```ros_send_point_cloud_topic``` 设置为发送点云的话题。 
+
+### 2.5 运行
+
+运行程序。
+
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_record_and_offline_decode_rosbag.md

@@ -0,0 +1,97 @@
+# How to record and decode rosbag
+
+## 1 Introduction
+
+This document will show you how to record and decode rosbag. Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/parameter_intro.md) before reading this document.
+
+## 2 Record
+
+### 2.1 Send packet to ROS
+
+We suppose you are connecting an online LiDAR and you have already sent out the point cloud to ROS.  If you have no idea about this, please read [Online connect lidar and send point cloud through ROS](how_to_online_send_point_cloud_ros.md) first.
+
+Actually, you can record the point cloud message now and you don't need to run the driver again when you off-line play the bag. But the disadvantage is also obvious that the point cloud message is very large. So normally we recommend to record packets rather than point cloud message. 
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: true                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap    
+```
+
+In order to record packets, set ```send_packet_ros = true```. 
+
+### 2.2 Record according to the topic
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets   
+  ros_send_point_cloud_topic: /rslidar_points      
+```
+
+User can also adjust the packets topic by adjust the ```ros_send_packet_topic``` in *lidar-ros* part of the config file. This topic represent the topic of the msop, and the topic of the difop will be ```msop-topic_difop```. e.g., the default topic value is set to ```rslidar_packets```, so the msop topic is ```rslidar_packets``` and the difop topic is ```rslidar_packets_difop```. The command to record bag is listed below. 
+
+```sh
+rosbag record /rslidar_packets /rslidar_packets_difop -O bag
+```
+
+**If you set send_packet_ros = true, both two kinds of packets will be sent to ROS. And you must record both of these two kinds of packets.**
+
+## 3 Offline Decode
+
+We suppose you have recorded a rosbag which contains msop packets with the topic ```rslidar_packets``` and difop packets with the topic ```rslidar_packets_difop```. 
+
+### 3.1 Set up the common part of the config file
+
+```yaml
+common:
+  msg_source: 2                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap   
+```
+
+Since the packets message come from the ROS, set ```msg_source = 2```. 
+
+We want to send point cloud to ROS so set ```send_point_cloud_ros = true```.
+
+### 3.2 Set up the lidar-driver part of the config file
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788           
+      start_angle: 0               
+      end_angle: 360              
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false 
+```
+
+Set the ```lidar_type```  to your LiDAR type.
+
+### 3.3 Set the lidar-ros part of the config file
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets   
+  ros_send_point_cloud_topic: /rslidar_points  
+```
+
+Set up the ```ros_recv_packet_topic```  to the ```msop``` topic in the rosbag.
+
+### 3.4 Run
+
+Run the program & play rosbag.
+
+ 

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_record_and_offline_decode_rosbag_cn.md

@@ -0,0 +1,99 @@
+# 如何录制与解码rosbag
+
+## 1 简介
+
+本文档将展示如何记录与解码rosbag。 在阅读这本文档之前请先阅读雷达用户手册与[参数简介](../intro/parameter_intro.md) 。
+
+## 2 录包
+
+### 2.1 将packet发送至ROS
+
+首先在线连接雷达并将点云发送至ROS。如果对此不太了解, 请先阅读 [在线连接雷达并发送点云到ROS](how_to_online_send_point_cloud_ros_cn.md) 。
+
+现在可以直接录制点云消息，这样在离线播包时不需要再另外运行驱动程序解包。但这种方法会导致录制的包非常大。 因此，通常建议记录雷达packet数据，而不是记录点云数据。
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: true                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap    
+```
+
+​	为了记录雷达packet, 需要设置 ```send_packet_ros = true```。
+
+### 2.2 根据对应话题录包
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets   
+  ros_send_point_cloud_topic: /rslidar_points      
+```
+
+用户可以通过调整参数文件的 *lidar-ros* 部分中的 ```ros_send_packet_topic``` 来调整发送的话题。 该话题表示msop的话题，而difop的话题为``` msop-topic_difop```。 例： 默认话题设置为 ```rslidar_packets```，因此msop话题为 ```rslidar_packets```，而difop的话题为 ```rslidar_packets_difop```。录包的指令如下所示。
+
+```bash
+rosbag record /rslidar_packets /rslidar_packets_difop -O bag
+```
+
+**如果将send_packet_ros设置为true，则两种数据包都将发送到ROS。 录包时必须同时记录这两种数据。**
+
+## 3 离线解码
+
+假设录制了一个rosbag，其中包含话题为 *rslidar_packets* 的msop数据包和话题为 *rslidar_packets_difop*的difop数据包。
+
+### 3.1 设置文件的 common部分
+
+```yaml
+common:
+  msg_source: 2                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap   
+```
+
+由于数据包消息来自ROS，因此设置 ```msg_source = 2``` 。
+
+将点云发送到ROS，因此设置 ```send_point_cloud_ros = true```。
+
+### 3.2 设置配置文件的lidar-driver部分
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788           
+      start_angle: 0               
+      end_angle: 360              
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false    
+```
+
+将 ```lidar_type``` 设置为LiDAR类型 。
+
+### 3.3 设置配置文件的lidar-ros部分
+
+```yaml
+ros:
+  ros_recv_packet_topic: /rslidar_packets    
+  ros_send_packet_topic: /rslidar_packets   
+  ros_send_point_cloud_topic: /rslidar_points  
+```
+
+将 ```ros_recv_packet_topic``` 设置为rosbag中的```msop```数据的话题。
+
+### 3.4 运行
+
+运行程序并播放rosbag。
+
+
+
+ 

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_switch_point_type.md

@@ -0,0 +1,52 @@
+# How to switch point type
+
+## 1 Introduction
+
+This document will show you how to switch the point type. The supported point type is listed in README. To switch the type, open the ```CMakeLists.txt``` in the project and check the top of the file. Remember to **rebuild** the project after changing the point type.
+
+```cmake
+#=======================================
+# Custom Point Type (XYZI, XYZIRT)
+#=======================================
+set(POINT_TYPE XYZI)
+```
+
+
+
+## 2 XYZI
+
+The default point type is the pcl official type```pcl::PointXYZI```.  Here is an example of transforming the ros point cloud to pcl point cloud. User can take this as an reference in their receiving program.
+
+```c++
+pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZI>);
+pcl::fromROSMsg(ros_msg, *cloud_ptr);
+```
+
+
+
+## 3 XYZIRT
+
+Since this is the custom pcl point type, user needs to add the definition of the point in the receiving program. The definition is shown below.
+
+```c++
+#include <pcl/io/io.h>
+#include <pcl/point_types.h>
+struct RsPointXYZIRT
+{
+  PCL_ADD_POINT4D;
+  uint8_t intensity;
+  uint16_t ring = 0;
+  double timestamp = 0;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp))
+
+```
+
+Then user can transform the ros point cloud to pcl point cloud.
+
+```c++
+pcl::PointCloud<RsPointXYZIRT>::Ptr cloud_ptr (new pcl::PointCloud<RsPointXYZIRT>);
+pcl::fromROSMsg(ros_msg, *cloud_ptr);
+```
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_switch_point_type_cn.md

@@ -0,0 +1,52 @@
+# 如何切换点的类型
+
+## 1 简介
+
+本文档将会介绍如何切换点的类型。目前支持的类型在README中有列出。切换点的类型时，首先打开项目根目录下的```CMakeLists.txt```文件，在文件顶部便可以选择点的类型。在改变类型后，需要重新编译整个工程。
+
+```cmake
+#=======================================
+# Custom Point Type (XYZI, XYZIRT)
+#=======================================
+set(POINT_TYPE XYZI)
+```
+
+
+
+## 2 XYZI
+
+默认的点的类型是pcl的官方类型```pcl::PointXYZI```. 此处给出了一个将ros点云消息转换成pcl点云的例子，用户在编写ros点云接收端时可以作为参考。
+
+```c++
+pcl::PointCloud<pcl::PointXYZI>::Ptr cloud_ptr (new pcl::PointCloud<pcl::PointXYZI>);
+pcl::fromROSMsg(ros_msg, *cloud_ptr);
+```
+
+
+
+## 3 XYZIRT
+
+由于这是速腾自定义的点的类型，用户需要在接收端程序添加对应的点的定义，点的定义如下所示。
+
+```c++
+#include <pcl/io/io.h>
+#include <pcl/point_types.h>
+struct RsPointXYZIRT
+{
+  PCL_ADD_POINT4D;
+  uint8_t intensity;
+  uint16_t ring = 0;
+  double timestamp = 0;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+} EIGEN_ALIGN16;
+POINT_CLOUD_REGISTER_POINT_STRUCT(RsPointXYZIRT, (float, x, x)(float, y, y)(float, z, z)(uint8_t, intensity, intensity)(uint16_t, ring, ring)(double, timestamp, timestamp))
+
+```
+
+然后即可调用pcl的转换函数将ros点云转换为pcl点云。
+
+```c++
+pcl::PointCloud<RsPointXYZIRT>::Ptr cloud_ptr (new pcl::PointCloud<RsPointXYZIRT>);
+pcl::fromROSMsg(ros_msg, *cloud_ptr);
+```
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_coordinate_transformation.md

@@ -0,0 +1,73 @@
+# How to use the coordinate transformation function
+
+## 1 Introduction
+
+ **rslidar_sdk** has the coordinate transformation function built inside and it can output the transformed point cloud directly, which can help users saving time to do transformation for point cloud (e.g. for RS128, it costs about 3~5ms to do transformation for one frame point cloud). This document will show you how to use the transformation function. 
+
+## 2 Dependency
+
+- Eigen3 
+
+  Installation：
+
+  ```bash
+  sudo apt-get install libeigen3-dev
+  ```
+
+## 3 Compile
+
+To enable the transformation function, the option ```ENABLE_TRANSFORM``` needs to be set to ```ON``` during the cmake process.
+
+- Compile directly
+
+  ```bash
+  cmake -DENABLE_TRANSFORM=ON ..
+  make -j4
+  ```
+
+- ROS
+
+  ```bash
+  catkin_make -DENABLE_TRANSFORM=ON
+  ```
+
+- ROS2
+
+  ```bash
+  colcon build --cmake-args '-DENABLE_TRANSFORM=ON'
+  ```
+
+## 4 Set up parameters
+
+User needs to set up the hiding parameter```x, y, z, roll, pitch ,yaw ``` in lidar part of the config.yaml. Please check the  [Intro to hiding parameters](../intro/hiding_parameters_intro.md) for more details. Here is an example of the config.yaml.
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699              
+      difop_port: 7788             
+      start_angle: 0               
+      end_angle: 360             
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false       
+	  x: 1
+	  y: 0
+	  z: 2.5
+	  roll: 0.1
+	  pitch: 0.2
+	  yaw: 1.57
+```
+
+## 5 Run
+
+Run the program.

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_coordinate_transformation_cn.md

@@ -0,0 +1,77 @@
+# 如何使用坐标变换功能
+
+## 1 简介
+
+**rslidar_sdk** 提供了内置的坐标变换功能，可以直接输出经过坐标变换后的点云，显著节省了用户对点云进行坐标变换的操作耗时（128线雷达一帧点云坐标变换耗时约3~5ms）。本文档将指导用户如何使用rslidar_sdk的内置坐标变换功能， 输出经过变换后的点云。 
+
+
+
+## 2 依赖介绍
+
+若希望启用坐标变换功能，需要安装以下依赖。
+
+- Eigen3 
+
+  安装方式：
+
+  ```bash
+  sudo apt-get install libeigen3-dev
+  ```
+
+## 3 编译
+
+若希望启用坐标变换的功能，在编译程序时需要将```ENABLE_TRANSFORM```选项设置为```ON```.
+
+- 直接编译
+
+  ```bash
+  cmake -DENABLE_TRANSFORM=ON ..
+  make -j4
+  ```
+
+- ROS
+
+  ```bash
+  catkin_make -DENABLE_TRANSFORM=ON
+  ```
+
+- ROS2
+
+  ```bash
+  colcon build --cmake-args '-DENABLE_TRANSFORM=ON'
+  ```
+
+## 4 参数设置
+
+用户需要设置lidar部分的隐藏参数```x, y, z, roll, pitch ,yaw ``` ，更多的细节可以参考[隐藏参数介绍](../intro/hiding_parameters_intro.md)。此处为参数文件的一个示例，用户可根据实际情况配置。
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699              
+      difop_port: 7788             
+      start_angle: 0               
+      end_angle: 360             
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false       
+	  x: 1
+	  y: 0
+	  z: 2.5
+	  roll: 0.1
+	  pitch: 0.2
+	  yaw: 1.57
+```
+
+## 5 运行
+
+运行程序。

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_multi_cast_function.md

@@ -0,0 +1,73 @@
+# How to use multi-cast function
+
+## 1 Introduction
+
+This document will show you how to use rslidar_sdk to receive point cloud from the LiDAR working in multi-cast mode.
+
+## 2 Steps (Linux)
+
+Suppose the multi-cast address of LiDAR is ```224.1.1.1```.  
+
+### 2.1 Set up hiding parameters
+
+User need to set up the hiding parameter ```multi_cast_address``` in lidar part of the config.yaml. Please check the  [Intro to hiding parameters](../intro/hiding_parameters_intro.md) for more details. Here is an example of the config.yaml.
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      multi_cast_address: 224.1.1.1
+      msop_port: 6699              
+      difop_port: 7788             
+      start_angle: 0               
+      end_angle: 360             
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false       
+
+```
+
+
+
+### 2.2 Set up the PC ip address
+
+In multi-cast case, the ip address of PC has no limit but user needs to make sure **the PC and LiDAR are in the same net work**, which means PC can ping to LiDAR.
+
+### 2.3 Add the PC to the group
+
+Use the following command to check the PC's ethernet card name:
+
+```bash
+ifconfig
+```
+
+![ethernet](../img/ethernet.png)
+
+As the figure shows, the ethernet card name is ```enp2s0```. Then execute the following command to add the PC to the group (replace the ```enp2s0``` with your ethernet card name):
+
+```
+sudo route add -net 224.0.0.0/4 dev enp2s0
+```
+
+### 2.4 Run
+
+Run the program. 
+
+
+
+
+
+
+
+
+
+
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_multi_cast_function_cn.md

@@ -0,0 +1,73 @@
+# 如何使用组播功能
+
+## 1 简介
+
+本文档将演示如何使用rslidar_sdk来接收组播模式下的雷达点云。
+
+## 2 步骤 (Linux)
+
+假设雷达的组播地址设置为```224.1.1.1```。
+
+### 2.1 设置隐藏参数
+
+用户首先需要将lidar部分的隐藏参数```multi_cast_address```设置为雷达的组播地址， 更多的细节可以参考[隐藏参数介绍](../intro/hiding_parameters_intro.md)，此处为参数文件的一个示例，用户可根据实际情况配置。
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      multi_cast_address: 224.1.1.1
+      msop_port: 6699              
+      difop_port: 7788             
+      start_angle: 0               
+      end_angle: 360             
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false       
+
+```
+
+
+
+### 2.2 设置电脑ip地址
+
+在组播模式下，电脑的ip地址并没有严格限制，但用户需要确保**电脑与雷达的网络互通**， 也就是电脑必须能够ping通雷达。
+
+### 2.3 将PC添加到组内
+
+使用以下指令查看电脑的网卡名:
+
+```bash
+ifconfig
+```
+
+![ethernet](../img/ethernet.png)
+
+如图所示，网卡名为```enp2s0```。 然后运行以下指令将电脑加入组内（将指令中的```enp2s0```替换为用户电脑的网卡名:
+
+```
+sudo route add -net 224.0.0.0/4 dev enp2s0
+```
+
+### 2.4 运行
+
+运行程序。 
+
+
+
+
+
+
+
+
+
+
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_multi_lidars.md

@@ -0,0 +1,215 @@
+# How to use multi-LiDARs
+
+## 1 Introduction
+
+This document will show you how to send out multi-LiDARs point cloud with only one driver running.  Theoretically, one driver can decoder unlimited number of LiDARs at the same time. For convenient, we will use three LiDARs as an example. Please make sure you have read the LiDAR user-guide and [Intro to parameters](../intro/parameter_intro.md) before reading this document.
+
+## 2 Online connect with multi-LiDARs
+
+### 2.1 Get the data port number
+
+Please follow the instructions in LiDAR user-guide to connect the LiDAR and set up your computer's ip address. At this time, you should have already known msop port number and difop port number for each LiDAR.  If you have no idea about what it is, please check the LiDAR user-guide first.
+
+### 2.2 Set up the common part of the config file
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+```
+
+Since the message come from the LiDAR, set ```msg_source = 1```. 
+
+Send point cloud to ROS so set ```send_point_cloud_ros = true```.
+
+### 2.3 Set up the lidar part of the config file
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /middle/rslidar_packets    
+      ros_send_packet_topic: /middle/rslidar_packets    
+      ros_send_point_cloud_topic: /middle/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 1990             
+      difop_port: 1991            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /left/rslidar_packets    
+      ros_send_packet_topic: /left/rslidar_packets    
+      ros_send_point_cloud_topic: /left/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60024                     
+      point_cloud_send_port: 60024                     
+      msop_recv_port: 60025                       
+      msop_send_port: 60025                       
+      difop_recv_port: 60026                      
+      difop_send_port: 60026       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1   
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 2010             
+      difop_port: 2011            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /right/rslidar_packets    
+      ros_send_packet_topic: /right/rslidar_packets    
+      ros_send_point_cloud_topic: /right/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60027                     
+      point_cloud_send_port: 60027                     
+      msop_recv_port: 60028                       
+      msop_send_port: 60028                       
+      difop_recv_port: 60029                      
+      difop_send_port: 60029       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+```
+
+Set ```lidar_type```  for each LiDAR.
+
+Set ```msop_port``` and ```difop_port```  for each LiDAR.
+
+Set ```ros_send_point_cloud_topic``` for each LiDAR.
+
+### 2.4 Run
+
+Run the demo.
+
+## 3 Offline use rosbag with multi-LiDARs
+
+### 3.1 Set up the common part of the config file
+
+```yaml
+common:
+  msg_source: 2                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap   
+```
+
+Since the packets message come from the ROS, set ```msg_source = 2```. 
+
+We want to send point cloud to ROS so set ```send_point_cloud_ros = true```.
+
+### 3.2 Set up the lidar part of the config file
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /middle/rslidar_packets    
+      ros_send_packet_topic: /middle/rslidar_packets    
+      ros_send_point_cloud_topic: /middle/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 1990             
+      difop_port: 1991            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /left/rslidar_packets    
+      ros_send_packet_topic: /left/rslidar_packets    
+      ros_send_point_cloud_topic: /left/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60024                     
+      point_cloud_send_port: 60024                     
+      msop_recv_port: 60025                       
+      msop_send_port: 60025                       
+      difop_recv_port: 60026                      
+      difop_send_port: 60026       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1   
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 2010             
+      difop_port: 2011            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /right/rslidar_packets    
+      ros_send_packet_topic: /right/rslidar_packets    
+      ros_send_point_cloud_topic: /right/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60027                     
+      point_cloud_send_port: 60027                     
+      msop_recv_port: 60028                       
+      msop_send_port: 60028                       
+      difop_recv_port: 60029                      
+      difop_send_port: 60029       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+```
+
+Set ```lidar_type```  for each LiDAR.
+
+Set the ```ros_recv_packet_topic``` for each LiDAR which need to corresbond to the topic names in rosbag.
+
+Set ```ros_send_point_cloud_topic``` for each LiDAR.
+
+### 3.3 Run
+
+Run the program & play rosbag.

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_multi_lidars_cn.md

@@ -0,0 +1,215 @@
+# 如何使用多雷达
+
+## 1 简介
+
+本文档将展示如何在仅运行一个驱动程序的情况解析并发送多台雷达的点云。理论上，一个驱动可以同时解码无限数量的雷达。为了方便起见，本文档将会使用三个雷达作为示例。在阅读本文档之前， 请确保已经阅读过雷达用户手册和[参数简介](../intro/parameter_intro.md) 。
+
+## 2 在线解析多雷达
+
+### 2.1 获取数据端口号
+
+首先将三个雷达与计算机正确连接，此时应已知每个LiDAR的msop端口号 与 difop端口号。  如果不清楚上述内容，请查看雷达用户手册。
+
+### 2.2 设置参数文件的common部分
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap     
+```
+
+由于消息来源于在线雷达，因此请设置```msg_source=1```。
+
+将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。
+
+### 2.3 设置配置文件中的lidar部分
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /middle/rslidar_packets    
+      ros_send_packet_topic: /middle/rslidar_packets    
+      ros_send_point_cloud_topic: /middle/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 1990             
+      difop_port: 1991            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /left/rslidar_packets    
+      ros_send_packet_topic: /left/rslidar_packets    
+      ros_send_point_cloud_topic: /left/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60024                     
+      point_cloud_send_port: 60024                     
+      msop_recv_port: 60025                       
+      msop_send_port: 60025                       
+      difop_recv_port: 60026                      
+      difop_send_port: 60026       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1   
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 2010             
+      difop_port: 2011            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /right/rslidar_packets    
+      ros_send_packet_topic: /right/rslidar_packets    
+      ros_send_point_cloud_topic: /right/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60027                     
+      point_cloud_send_port: 60027                     
+      msop_recv_port: 60028                       
+      msop_send_port: 60028                       
+      difop_recv_port: 60029                      
+      difop_send_port: 60029       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+```
+
+为每个雷达类型设置型号```lidar_type```。
+
+为每个雷达设置对应的端口号 ```msop_port``` 和```difop_port``` 。
+
+为每个雷达设置点云发送的话题```ros_send_point_cloud_topic```。
+
+### 2.4 运行
+
+运行程序。
+
+## 3 离线解析多雷达rosbag
+
+### 3.1 设置配置文件中的common部分
+
+```yaml
+common:
+  msg_source: 2                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap   
+```
+
+由于数据包消息来自ROS，因此设置 ```msg_source = 2``` 。
+
+将点云发送到ROS，因此设置 ```send_point_cloud_ros = true```。
+
+### 3.2 设置配置文件中的lidar部分
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /middle/rslidar_packets    
+      ros_send_packet_topic: /middle/rslidar_packets    
+      ros_send_point_cloud_topic: /middle/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 1990             
+      difop_port: 1991            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /left/rslidar_packets    
+      ros_send_packet_topic: /left/rslidar_packets    
+      ros_send_point_cloud_topic: /left/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60024                     
+      point_cloud_send_port: 60024                     
+      msop_recv_port: 60025                       
+      msop_send_port: 60025                       
+      difop_recv_port: 60026                      
+      difop_send_port: 60026       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1   
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 2010             
+      difop_port: 2011            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /right/rslidar_packets    
+      ros_send_packet_topic: /right/rslidar_packets    
+      ros_send_point_cloud_topic: /right/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60027                     
+      point_cloud_send_port: 60027                     
+      msop_recv_port: 60028                       
+      msop_send_port: 60028                       
+      difop_recv_port: 60029                      
+      difop_send_port: 60029       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+```
+
+为每个雷达类型设置型号```lidar_type```。
+
+为每个雷达设置接收的packet话题名```ros_recv_packet_topic```。
+
+为每个雷达设置点云发送的话题```ros_send_point_cloud_topic```。
+
+### 3.3 运行
+
+运行程序并播放rosbag。

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_protobuf_function.md

@@ -0,0 +1,224 @@
+# How to use protobuf functions to send & receive message
+
+## 1 Introduction
+
+Suppose there are two computers, PC-A and PC-B and they are far away from each other.  You connect LiDAR with PC-A and for some reasons, you want to use point cloud message in PC-B. At this time, you may need to use the protobuf functions. Typically, there are two ways to achieve this goal.
+
+- PC-A send out the packets message to PC-B. PC-B receive the packet message and decode it , then PC-B get the point cloud message and use it.
+
+- PC-A decode the packets message, get the point cloud and send out the point cloud message to PC-B. PC-B receive the point cloud message and use it directly.
+
+We offer both of these two ways but we recommend first method rather than second method because the point cloud message is very large and it may take up your bandwidth.  
+
+## 2 Send & Receive packets through Protobuf-UDP
+
+ We suppose you have already read [Intro to parameters](../intro/parameter_intro.md) and you already have a basic idea about the config file. 
+
+### 2.1 PC-A(Sender)
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: false                            
+  send_packet_proto: true                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+Since the message come from the LiDAR, set ```msg_source = 1```. 
+
+Send packets through Protobuf-UDP so set ```send_packet_proto = true```.
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1  
+```
+
+Set the ```lidar_type```  to your LiDAR type.
+
+Set the ```msop_port``` and ```difop_port```  to your LiDAR's port number. 
+
+Set the ```msop_send_port```, ```difop_send_port```, and ```packet_send_ip```.
+
+### 2.2 PC-B(Receiver)
+
+```yaml
+common:
+  msg_source: 4                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+Since the packets message come from protobuf-UDP, set ```msg_source = 4```.
+
+Send point cloud to ROS so set ```send_point_cloud_ros = true```. 
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1  
+```
+
+Set the ```lidar_type```  to your LiDAR type.
+
+Make sure the PC-B's ip address is same as the ```packet_send_ip``` set in PC-A's config.yaml.
+
+Set the ```msop_recv_port``` and  ```difop_recv_port``` to be same with the  ```msop_send_port``` and  ```difop_send_port``` set in PC-A's config.yaml.
+
+## 3 Send & receive point cloud through Protobuf-UDP
+
+We suppose you have already read [Intro to parameters](../intro/parameter_intro.md) and you already have a basic idea about the config file. 
+
+### 3.1 PC-A(Sender)
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: false                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: true                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+Since the message come from the LiDAR, set ```msg_source = 1```. 
+
+Send point cloud through Protobuf-UDP so set ```send_point_cloud_proto = true```.
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1  
+```
+
+Set the ```lidar_type```  to your LiDAR type.
+
+Set the ```msop_port``` and ```difop_port```  to your LiDAR's port number. 
+
+Set the ```point_cloud_send_port``` and ```point_cloud_send_ip``.
+
+### 3.2 PC-B(Receiver)
+
+```yaml
+common:
+  msg_source: 5                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+Since the point cloud message come from protobuf-UDP, set ```msg_source = 5```.
+
+Send point cloud to ROS so set ```send_point_cloud_ros = true```. 
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1
+```
+
+Make sure the PC-B's ip address is same as the ```point_cloud_send_ip``` set in PC-A's config.yaml.
+
+Set the ```point_cloud_recv_port```  to be same with the  ```point_cloud_send_port```  set in PC-A's config.yaml.
+
+
+
+
+
+
+
+
+
+
+

src/ros/open_source_code/rslidar_sdk/doc/howto/how_to_use_protobuf_function_cn.md

@@ -0,0 +1,228 @@
+# 如何使用Protobuf函数发送和接收消息
+
+## 1 简介
+
+假设有两台计算机，PC-A和PC-B，并且它们彼此相距很远。 将LiDAR与PC-A连接，由于某些原因，用户想在PC-B中使用点云消息。 此时，可能需要使用protobuf功能。 通常，有两种方法可以实现此目标。
+
+- PC-A将雷达packet消息发送到PC-B。 PC-B收到雷达packet消息并对其进行解码，然后PC-B获得点云消息并使用它。
+
+- PC-A解码雷达packet消息，获取点云并将点云消息发送到PC-B。 PC-B收到点云消息并直接使用。
+
+rslidar_sdk提供这两种方式，但是通常建议使用第一种方法，因为点云消息非常大，对带宽有较高要求。  
+
+## 2 通过Protobuf-UDP发送和接收 packets
+
+​	首先请阅读[参数简介](.. / intro / parameter_intro.md)，了解基本的参数配置。 
+
+### 2.1 PC-A(发送端)
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: false                            
+  send_packet_proto: true                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+由于消息来源于在线雷达，因此请设置```msg_source=1```。
+
+将packet数据通过Protobuf-UDP发出，因此设置 ```send_packet_proto = true``` 。
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1  
+```
+
+将 ```lidar_type``` 设置为LiDAR类型 。
+
+
+设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。
+
+设置 ```msop_send_port```, ```difop_send_port```, 和 ```packet_send_ip```.
+
+### 2.2 PC-B(接收端)
+
+```yaml
+common:
+  msg_source: 4                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+由于packet消息来源于protobuf-UDP，因此设置 ```msg_source = 4``` 。
+
+将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1  
+```
+
+将 ```lidar_type``` 设置为LiDAR类型 。
+
+确认PC-B的ip地址与PC-A的配置文件中设置的```packet_send_ip```一致。
+
+将```msop_recv_port```和```difop_recv_port```与PC-A的配置文件中设置的```msop_send_port```和```difop_send_port```设置为一致。
+
+## 3 通过Protobuf-UDP发送和接收点云
+
+首先请阅读[参数简介](... / intro / parameter_intro.md)，了解基本的参数配置。 
+
+### 3.1 PC-A(发送端)
+
+```yaml
+common:
+  msg_source: 1                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: false                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: true                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+由于消息来源于在线雷达，因此请设置```msg_source=1```。
+
+将点云数据通过Protobuf-UDP发出，因此设置 ```send_point_cloud_proto = true``` 。
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1  
+```
+
+将 ```lidar_type``` 设置为LiDAR类型 。
+
+
+设置 ```msop_port``` 和 ```difop_port``` 为雷达数据端口号。
+
+设置 ```point_cloud_send_port``` 和 ```point_cloud_send_ip```.
+
+### 3.2 PC-B(接收端)
+
+```yaml
+common:
+  msg_source: 5                                       
+  send_packet_ros: false                                
+  send_point_cloud_ros: true                            
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+```
+
+由于点云消息来源于protobuf-UDP，因此设置 ```msg_source = 5``` 。
+
+将点云发送到ROS以查看，因此设置 ```send_point_cloud_ros = true``` 。
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1  
+```
+
+确认PC-B的ip地址与PC-A的配置文件中设置的```point_cloud_send_ip```一致。
+
+将```point_cloud_recv_port```与PC-A的配置文件中设置的```point_cloud_send_port```设置为一致。
+
+
+
+
+
+
+
+
+
+
+
+
+

src/ros/open_source_code/rslidar_sdk/doc/intro/hiding_parameters_intro.md

@@ -0,0 +1,67 @@
+# Introduction to hidden parameters
+
+In order to make the config file as simple as possible, we selectively hide some of the parameters and give them a default value in the program. If not added to  ```config.yaml```, the default values will be used. This document explains the meanings of these these hidden parameters. 
+
+
+
+## 1 common
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                                
+  send_point_cloud_ros: false                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap                 
+  pcap_repeat: true									    
+  pcap_rate: 1  											
+```
+
+- ```pcap_repeat``` -- The default value is ```true``` , you can add it back and set to false to prevent pcap read repeatedly.
+
+- ```pcap_rate``` -- The default value is ```1``` and the point cloud frequency is about 10hz. The larger the value is set, the faster the pcap bag is played.
+
+
+
+## 2 lidar
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699              
+      difop_port: 7788             
+      start_angle: 0               
+      end_angle: 360             
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false       
+      angle_path: /home/robosense/angle.csv   
+      split_frame_mode: 1	      
+      cut_angle: 0   
+	  num_pkts_split: 1 	                    
+      wait_for_difop: true         
+      saved_by_rows: false
+      multi_cast_address: 0.0.0.0
+      x: 0
+      y: 0
+      z: 0
+      roll: 0
+      pitch: 0
+      yaw: 0
+```
+
+- ```angle_path``` -- The path of the angle.csv. For latest version of LiDARs, this parameter can be ignored.
+- ```split_frame_mode``` -- The mode to split the LiDAR frames. Default value is ```1```.
+  - 1 -- Spliting frames depending on the cut_angle
+  - 2 -- Spliting frames depending on a fixed number of packets
+  - 3 -- Spliting frames depending on num_pkts_split
+- ```cut_angle``` --  The angle(degree) to split frames. Only be used when ```split_frame_mode = 1```. The default value is ```0```.
+- ```num_pkts_split``` -- The number of packets in one frame. Only be used when ```split_frame_mode = 3```.
+- ```wait_for_difop``` -- If set to false, the driver will not wait for difop packet and send out the point cloud immediately. The default value is ```true```.
+- ```saved_by_rows``` --  The default point cloud is stored in **column major order**, which means if there is  a point msg.point_cloud_ptr->at(i) , the next point on the same ring should be msg.point_cloud_ptr->at(i+msg.height). If this parameter is set to  ```true``` , the point cloud will be stored in **row major order**.
+- ```multi_cast_address``` -- If use multi-cast function, this parameter need to be set correctly. For more details, please refer to  [Multi-Cast](../howto/how_to_use_multi_cast_function.md) 
+
+- ```x, y, z, roll, pitch, yaw ``` -- The parameters to do cooridiante transformation. If the coordinate transformation function is enabled in driver core,  the output point cloud will be transformed based on these parameters. For more details, please refer to [Coordinate Transformation](../howto/how_to_use_coordinate_transformation.md) 

src/ros/open_source_code/rslidar_sdk/doc/intro/hiding_parameters_intro_cn.md

@@ -0,0 +1,65 @@
+# 隐藏参数介绍
+
+为了让参数配置文件尽可能简洁，我们选择隐藏了部分用户不常用到的参数并在程序内给予了它们默认值。 本文档将详细介绍这些隐藏参数，用户可自行选择是否要将它们添加回参数文件内。
+
+## 1 common
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                                
+  send_point_cloud_ros: false                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap                 
+  pcap_repeat: true									    
+  pcap_rate: 1  											
+```
+
+- ```pcap_repeat``` -- 默认值为true， 用户可将其设置为false来禁用pcap循环播放功能。
+
+- ```pcap_rate``` -- 默认值为1，点云频率约为10hz。 用户可调节此参数来控制pcap播放速度，设置的值越大，pcap播放速度越快。
+
+
+
+## 2 lidar
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128            
+      frame_id: /rslidar           
+      msop_port: 6699              
+      difop_port: 7788             
+      start_angle: 0               
+      end_angle: 360             
+      min_distance: 0.2            
+      max_distance: 200           
+      use_lidar_clock: false       
+      angle_path: /home/robosense/angle.csv   
+      split_frame_mode: 1	      
+      cut_angle: 0   
+	  num_pkts_split: 1 	                    
+      wait_for_difop: true         
+      saved_by_rows: false
+      multi_cast_address: 0.0.0.0
+      x: 0
+      y: 0
+      z: 0
+      roll: 0
+      pitch: 0
+      yaw: 0
+```
+
+- ```angle_path``` -- angle.csv外参文件的路径，仅用于调试，可忽略。
+- ```split_frame_mode``` -- 分帧模式设置，默认值为```1```。
+  - 1 -- 角度分帧
+  - 2 -- 固定包数分帧
+  - 3 -- 自定义包数分帧
+- ```cut_angle``` --  用于分帧的角度(单位为度)， 在```split_frame_mode = 1``` 时才生效，默认值为```0```。
+- ```num_pkts_split``` -- 用于分帧的包数，在 ```split_frame_mode = 3```时才生效，默认值为1。
+- ```wait_for_difop``` -- 若设置为false， 驱动将不会等待difop包而是立即解析并发出点云。 默认值为```true```，也就是必须要有difop包才会进行点云解析。
+- ```saved_by_rows``` --  点云的默认储存方式为```按列储存```，也就是说假设有一个点msg.point_cloud_ptr->at(i) ，那么与这个点同一行的下一个点应该为msg.point_cloud_ptr->at(i+msg.height)。如果此参数设置为```true```,那么输出的点云将会```按行储存```。
+- ```multi_cast_address``` -- 如果雷达为组播模式，此参数需要被设置为组播的地址。具体使用方式可以参考 [组播模式](../howto/how_to_use_multi_cast_function_cn.md) 
+
+- ```x, y, z, roll, pitch, yaw ``` -- 坐标变换参数，若启用了内核的坐标变换功能，将会使用此参数输出经过变换后的点云。x, y, z, 单位为```米```, roll, pitch, yaw, 单位为```弧度```。具体使用方式可以参考 [坐标变换功能](../howto/how_to_use_coordinate_transformation_cn.md) 

src/ros/open_source_code/rslidar_sdk/doc/intro/parameter_intro.md

@@ -0,0 +1,251 @@
+# Parameters Introduction
+
+There is only one configure file ```config.yaml```, which is stored in ```rslidar_sdk/config```.  The ```config.yaml``` can be divided into two parts, the common part  and the lidar part . 
+
+*In multi-LiDARs case, the parameters in common part will be shared by all LiDARs, while the parameters in lidar part need to be adjust for each LiDAR.*
+
+**config.yaml is indentation sensitive! Please make sure the indentation is not changed when adjusting the parameters!**
+
+
+
+## 1 Common
+
+This part is used to decide the source of LiDAR data, and whether to publish point clouds or not.
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                               
+  send_point_cloud_ros: false                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap                 
+```
+
+- msg_source
+
+  - 0 -- Not use lidar. Basically you will never set this parameter to 0.
+
+  - 1 -- When connecting with a running lidar, set to 1. For more details, please refer to [Online connect lidar and send point cloud through ROS](../howto/how_to_online_send_point_cloud_ros.md)
+
+  - 2 -- The lidar packet come from ROS or ROS2. This will be used in offline decode rosbag.  For more details, please refer to [Record rosbag & Offline decode rosbag](../howto/how_to_record_and_offline_decode_rosbag.md)
+
+  - 3 -- The lidar packet come from offline pcap bag. For more details, please refer to  [Decode pcap bag and send point cloud through ROS](../howto/how_to_offline_decode_pcap.md)
+
+  - 4 -- The lidar packet come from Protobuf-UDP. For more details, please refer to [Use protobuf send & receive](../howto/how_to_use_protobuf_function.md)
+
+  - 5 -- The lidar point cloud come from Protobuf-UDP. For more details, please refer to  [Use protobuf send & receive](../howto/how_to_use_protobuf_function.md)
+
+- send_packet_ros
+
+   - true -- The lidar packets will be sent to ROS or ROS2. 
+
+   
+*Since the ROS packet message is of a customized message type, you can't directly echo the topic through ROS. Mostly the packets are only used to record offline bag because the size is much smaller than point cloud.*
+
+- send_point_cloud_ros
+
+   - true -- The lidar point cloud will be sent to ROS or ROS2. 
+   
+   
+   *The ROS point cloud type is the ROS official defined type -- sensor_msgs/PointCloud2, which means the point cloud can be visualized on ROS-Rviz directly. Also you can record the point cloud to rosbag but its size may be very large, that's why we suggest to  record packets.*
+   
+- send_packet_proto
+
+   - true -- The lidar packets will be sent out as protobuf message through ethernet by UDP protocal.
+   
+- send_point_cloud_proto
+
+   - true -- The lidar point cloud will be sent out as protobuf message through ethernet in UDP protocal. 
+   
+
+*We suggest sending packets rather than point clouds through ethernet by protobuf  because point cloud size is too large which may take up a lot of bandwidth.*
+
+- pcap_path
+
+   If the msg_source = 3, please make sure the pcap_path is correct, otherwise this paramter can be igonred.
+
+
+
+## 2 lidar
+
+This part needs to be adjusted for every LiDAR seperately (in multi-LiDARs case). 
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1                   
+```
+
+- lidar_type
+
+  Supported types of LiDAR are listed in README.
+
+- frame_id
+
+  The frame id of the point cloud message.
+
+- msop_port, difop_port
+
+  The msop port and difop_port of LiDAR. *Please check these parameters first is no data received.*
+
+- start_angle, end_angle
+
+  The start angle and end angle of the point cloud, which should be set in range of 0~360°. (*start_angle **can** be larger than end_angle*).
+
+- min_distance, max_distance
+
+  The minimum distance and maximum distance of the point cloud. 
+
+- use_lidar_clock
+
+  - true -- Use the lidar internal clock as the message timestamp
+  - false -- Use the system clock as the message timestamp
+
+
+
+## 3 Example
+
+Here are two examples. The first configure file is for single LiDAR case and second is used for the case where there are 3 LiDARs. Please adjust the specific parameters according to your own case.
+
+- Online connection to single LiDAR & Send point cloud to ROS
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                               
+  send_point_cloud_ros: true                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1                
+```
+
+- Online connection to three LiDARs & Send point cloud to ROS
+
+*Pay attention to the indentation of lidar part*
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                               
+  send_point_cloud_ros: true                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /middle/rslidar_packets    
+      ros_send_packet_topic: /middle/rslidar_packets    
+      ros_send_point_cloud_topic: /middle/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 1990             
+      difop_port: 1991            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /left/rslidar_packets    
+      ros_send_packet_topic: /left/rslidar_packets    
+      ros_send_point_cloud_topic: /left/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60024                     
+      point_cloud_send_port: 60024                     
+      msop_recv_port: 60025                       
+      msop_send_port: 60025                       
+      difop_recv_port: 60026                      
+      difop_send_port: 60026       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1   
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 2010             
+      difop_port: 2011            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /right/rslidar_packets    
+      ros_send_packet_topic: /right/rslidar_packets    
+      ros_send_point_cloud_topic: /right/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60027                     
+      point_cloud_send_port: 60027                     
+      msop_recv_port: 60028                       
+      msop_send_port: 60028                       
+      difop_recv_port: 60029                      
+      difop_send_port: 60029       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+```
+

src/ros/open_source_code/rslidar_sdk/doc/intro/parameter_intro_cn.md

@@ -0,0 +1,240 @@
+# 参数介绍
+
+本工程只有一份参数文件 ```config.yaml```， 储存于```rslidar_sdk/config```文件夹内。 整个参数文件可以被分为两部分，common部分以及lidar部分。 *在多雷达情况下，common部分的参数设置将会被所有雷达共享，而lidar部分需要根据每台雷达实际情况分别进行设置。*
+
+**参数文件config.yaml对缩进有严格的要求！请确保修改参数之后每行开头的缩进仍保持一致！**
+
+## 1 Common
+
+此部分用于设置雷达的消息来源，以及是否将结果发布。
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                               
+  send_point_cloud_ros: false                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap                 
+```
+
+- msg_source
+
+  - 1 -- 连接在线雷达. 更多使用细节请参考[在线读取雷达数据发送到ROS](../howto/how_to_online_send_point_cloud_ros_cn.md)。
+
+  - 2 -- 离线解析ROS或ROS2的packet包。更多使用细节请参考 [录制ROS数据包&离线解析ROS数据包](doc/howto/how_to_record_and_offline_decode_rosbag_cn.md)。
+
+  - 3 -- 离线解析pcap包。更多使用细节请参考[离线解析Pcap包发送到ROS](doc/howto/how_to_offline_decode_pcap_cn.md)。
+
+  - 4 -- 雷达消息来源为Protobuf-UDP的packet消息，更多使用细节请参考 [使用Protobuf发送&接收](../howto/how_to_use_protobuf_function_cn.md)。
+
+  - 5 -- 雷达消息来源为Protobuf-UDP的点云消息，更多使用细节请参考 [使用Protobuf发送&接收](../howto/how_to_use_protobuf_function_cn.md)。
+
+- send_packet_ros
+
+   - true -- 雷达packet消息将通过ROS或ROS2发出 
+
+     *由于雷达ROS packet消息为速腾聚创自定义ROS消息，因此用户无法直接echo话题查看消息具体内容。实际上packet主要用于录制离线ROS包，因为packet的体积小于点云。*
+   
+- send_point_cloud_ros
+
+   - true -- 雷达点云消息将通过ROS或ROS2发出 
+
+   *点云消息类型为ROS官方定义的点云类型sensor_msgs/PointCloud2, 因此用户可以直接使用Rviz查看点云。同时，用户也可以选择录包时直接录制点云，但这样做包的体积会非常大，因此我们建议离线录制ROS包时录制packet消息。*
+
+- send_packet_proto
+
+   - true -- 雷达packet消息将通过Protobuf-UDP发出
+
+- send_point_cloud_proto
+
+   - true -- 雷达点云消息将通过Protobuf-UDP发出
+
+   *我们建议发送packet消息而不是点云，因为点云消息体积过大，对带宽有较高的要求。.*
+
+- pcap_path
+
+   如果msg_dource = 3, 请确保此参数设置为正确的pcap包的路径。
+
+
+
+## 2 lidar
+
+本部分需要根据不同的雷达进行设置（多雷达时）。
+
+```yaml
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1                   
+```
+
+- lidar_type
+
+  目前支持的雷达型号已在README中列出。
+
+- frame_id
+
+  点云消息的frame_id。
+
+- msop_port, difop_port
+
+  点云的msop端口号和difop端口号。 *若收不到消息，请优先确认这两个参数是否配置正确。*
+
+- start_angle, end_angle
+
+  点云消息的起始角度和结束角度，此处设置为软件屏蔽，无法减小每帧点云的体积，只会将区域外的点设置为NAN点。 起始角和结束角的范围应在0~360°之间。(**起始角可以大于结束角**).
+
+- min_distance, max_distance
+
+  点云的最小距离和最大距离，此处设置为软件屏蔽，无法减小每帧点云的体积，只会将区域外的点设置为NAN点。
+
+- use_lidar_clock
+
+  - true -- 使用雷达时间作为消息时间戳。
+  - false -- 使用系统时间作为消息时间戳。 
+
+
+
+## 3 示例
+
+在线连接一台雷达，并发送点云到ROS。
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                               
+  send_point_cloud_ros: true                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /rslidar_packets    
+      ros_send_packet_topic: /rslidar_packets    
+      ros_send_point_cloud_topic: /rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1                
+```
+
+在线连接3台雷达，并发送点云到ROS。
+
+*注意lidar部分参数的缩进*
+
+```yaml
+common:
+  msg_source: 1                                         
+  send_packet_ros: false                               
+  send_point_cloud_ros: true                           
+  send_packet_proto: false                              
+  send_point_cloud_proto: false                         
+  pcap_path: /home/robosense/lidar.pcap 
+lidar:
+  - driver:
+      lidar_type: RS128           
+      frame_id: /rslidar           
+      msop_port: 6699             
+      difop_port: 7788            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /middle/rslidar_packets    
+      ros_send_packet_topic: /middle/rslidar_packets    
+      ros_send_point_cloud_topic: /middle/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60021                     
+      point_cloud_send_port: 60021                     
+      msop_recv_port: 60022                       
+      msop_send_port: 60022                       
+      difop_recv_port: 60023                      
+      difop_send_port: 60023       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 1990             
+      difop_port: 1991            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /left/rslidar_packets    
+      ros_send_packet_topic: /left/rslidar_packets    
+      ros_send_point_cloud_topic: /left/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60024                     
+      point_cloud_send_port: 60024                     
+      msop_recv_port: 60025                       
+      msop_send_port: 60025                       
+      difop_recv_port: 60026                      
+      difop_send_port: 60026       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1   
+  - driver:
+      lidar_type: RSBP           
+      frame_id: /rslidar           
+      msop_port: 2010             
+      difop_port: 2011            
+      start_angle: 0              
+      end_angle: 360               
+      min_distance: 0.2            
+      max_distance: 200            
+      use_lidar_clock: false        
+    ros:
+      ros_recv_packet_topic: /right/rslidar_packets    
+      ros_send_packet_topic: /right/rslidar_packets    
+      ros_send_point_cloud_topic: /right/rslidar_points      
+    proto:
+      point_cloud_recv_port: 60027                     
+      point_cloud_send_port: 60027                     
+      msop_recv_port: 60028                       
+      msop_send_port: 60028                       
+      difop_recv_port: 60029                      
+      difop_send_port: 60029       
+      point_cloud_send_ip: 127.0.0.1                   
+      packet_send_ip: 127.0.0.1    
+```
+

src/ros/open_source_code/rslidar_sdk/launch/start.launch

@@ -0,0 +1,6 @@
+<launch>
+  <node pkg="rslidar_sdk" name="rslidar_sdk_node" type="rslidar_sdk_node" output="screen">
+  </node>
+  <!-- rviz -->
+  <node pkg="rviz" name="rviz" type="rviz" args="-d $(find rslidar_sdk)/rviz/rviz.rviz" />
+</launch>

src/ros/open_source_code/rslidar_sdk/launch/start.py

@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from ament_index_python.packages import get_package_share_directory
+def generate_launch_description():
+    rviz_config=get_package_share_directory('rslidar_sdk')+'/rviz/rviz2.rviz'
+    return LaunchDescription([
+        Node(
+            package='rslidar_sdk',
+            node_namespace='rslidar_sdk',
+            node_executable='rslidar_sdk_node',
+            output='screen',
+            node_name='rslidar_sdk_node'
+        ),
+        Node(
+            package='rviz2',
+            node_namespace='rviz2',
+            node_executable='rviz2',
+            node_name='rviz2',
+            arguments=['-d',rviz_config]
+        )
+    ])

src/ros/open_source_code/rslidar_sdk/node/rslidar_sdk_node.cpp

@@ -0,0 +1,88 @@
+/*********************************************************************************************************************
+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 <signal.h>
+#include "manager/adapter_manager.h"
+using namespace robosense::lidar;
+std::mutex g_mtx;
+std::condition_variable g_cv;
+static void sigHandler(int sig)
+{
+  RS_MSG << "RoboSense-LiDAR-Driver is stopping....." << RS_REND;
+#ifdef ROS_FOUND
+  ros::shutdown();
+#endif
+  g_cv.notify_all();
+}
+
+int main(int argc, char** argv)
+{
+  signal(SIGINT, sigHandler);  ///< bind ctrl+c signal with the sigHandler function
+  RS_TITLE << "********************************************************" << RS_REND;
+  RS_TITLE << "**********                                    **********" << RS_REND;
+  RS_TITLE << "**********    RSLidar_SDK Version: v" << RSLIDAR_VERSION_MAJOR << "." << RSLIDAR_VERSION_MINOR << "."
+           << RSLIDAR_VERSION_PATCH << "     **********" << RS_REND;
+  RS_TITLE << "**********                                    **********" << RS_REND;
+  RS_TITLE << "********************************************************" << RS_REND;
+
+  std::shared_ptr<AdapterManager> demo_ptr = std::make_shared<AdapterManager>();
+  YAML::Node config;
+  try
+  {
+    config = YAML::LoadFile((std::string)PROJECT_PATH + "/config/config.yaml");
+  }
+  catch (...)
+  {
+    RS_ERROR << "Config file format wrong! Please check the format(e.g. indentation) " << RS_REND;
+    return -1;
+  }
+
+#ifdef ROS_FOUND  ///< if ROS is found, call the ros::init function
+  ros::init(argc, argv, "rslidar_sdk_node", ros::init_options::NoSigintHandler);
+#endif
+
+#ifdef ROS2_FOUND  ///< if ROS2 is found, call the rclcpp::init function
+  rclcpp::init(argc, argv);
+#endif
+
+  demo_ptr->init(config);
+  demo_ptr->start();
+  RS_MSG << "RoboSense-LiDAR-Driver is running....." << RS_REND;
+
+#ifdef ROS_FOUND
+  ros::spin();
+#else
+  std::unique_lock<std::mutex> lck(g_mtx);
+  g_cv.wait(lck);
+#endif
+  return 0;
+}

src/ros/open_source_code/rslidar_sdk/package.xml

@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<package>
+  <name>rslidar_sdk</name>
+  <version>1.3.0</version>
+  <description>The rslidar_sdk package</description>
+  <maintainer email="zdxiao@robosense.cn">robosense</maintainer>
+  <license>BSD</license>
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>roscpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+</package>

src/ros/open_source_code/rslidar_sdk/package_ros2.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>rslidar_sdk</name>
+  <version>1.3.0</version>
+  <description>The rslidar_sdk package</description>
+  <maintainer email="zdxiao@robosense.cn">robosense</maintainer>
+  <license>BSD</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rclcpp</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <exec_depend>rclcpp</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <depend>rslidar_msg</depend>
+  <export>
+     <build_type>ament_cmake</build_type>
+  </export>
+  
+</package>
+
+
+

src/ros/open_source_code/rslidar_sdk/rviz/rviz2.rviz

@@ -0,0 +1,154 @@
+Panels:
+  - Class: rviz_common/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded: ~
+      Splitter Ratio: 0.5
+    Tree Height: 796
+  - Class: rviz_common/Selection
+    Name: Selection
+  - Class: rviz_common/Tool Properties
+    Expanded:
+      - /2D Goal Pose1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz_common/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz_default_plugins/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 30.831819534301758
+        Min Value: -6.263338565826416
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz_default_plugins/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 241
+      Min Color: 0; 0; 0
+      Min Intensity: 1
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.05000000074505806
+      Style: Points
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rslidar_points
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Fixed Frame: rslidar
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz_default_plugins/Interact
+      Hide Inactive Objects: true
+    - Class: rviz_default_plugins/MoveCamera
+    - Class: rviz_default_plugins/Select
+    - Class: rviz_default_plugins/FocusCamera
+    - Class: rviz_default_plugins/Measure
+      Line color: 128; 128; 0
+    - Class: rviz_default_plugins/SetInitialPose
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /initialpose
+    - Class: rviz_default_plugins/SetGoal
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /goal_pose
+    - Class: rviz_default_plugins/PublishPoint
+      Single click: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /clicked_point
+  Transformation:
+    Current:
+      Class: rviz_default_plugins/TF
+  Value: true
+  Views:
+    Current:
+      Class: rviz_default_plugins/Orbit
+      Distance: 73.69597625732422
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Focal Point:
+        X: 0.3094900846481323
+        Y: 6.032918930053711
+        Z: -1.6452505588531494
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.505397617816925
+      Target Frame: <Fixed Frame>
+      Value: Orbit (rviz)
+      Yaw: 3.0422189235687256
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1025
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003a7fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003a7000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003a7fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003a7000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004cc000003a700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1853
+  X: 67
+  Y: 27

src/ros/open_source_code/rslidar_sdk/src/adapter/adapter_base.hpp

@@ -0,0 +1,148 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#include "utility/common.h"
+#include "utility/yaml_reader.hpp"
+#include "rs_driver/msg/packet_msg.h"
+#include "rs_driver/msg/scan_msg.h"
+#include "msg/rs_msg/lidar_point_cloud_msg.h"
+
+namespace robosense
+{
+namespace lidar
+{
+enum MsgSource
+{
+  MSG_FROM_LIDAR = 1,
+  MSG_FROM_ROS_PACKET = 2,
+  MSG_FROM_PCAP = 3,
+  MSG_FROM_PROTO_PACKET = 4,
+  MSG_FROM_PROTO_POINTCLOUD = 5
+};
+enum class AdapterType
+{
+  DriverAdapter,
+  PointCloudRosAdapter,
+  PointCloudProtoAdapter,
+  PacketRosAdapter,
+  PacketProtoAdapter,
+  CameraTriggerRosAdapter
+};
+class AdapterBase
+{
+public:
+  typedef std::shared_ptr<AdapterBase> Ptr;
+  AdapterBase() = default;
+  virtual ~AdapterBase();
+  virtual void init(const YAML::Node& config) = 0;
+  virtual void start();
+  virtual void stop();
+  virtual void sendScan(const ScanMsg& msg);
+  virtual void sendPacket(const PacketMsg& msg);
+  virtual void sendPointCloud(const LidarPointCloudMsg& msg);
+  virtual void sendCameraTrigger(const CameraTrigger& msg);
+  virtual void regRecvCallback(const std::function<void(const ScanMsg&)>& callback);
+  virtual void regRecvCallback(const std::function<void(const PacketMsg&)>& callback);
+  virtual void regRecvCallback(const std::function<void(const LidarPointCloudMsg&)>& callback);
+  virtual void regRecvCallback(const std::function<void(const CameraTrigger&)>& callback);
+  virtual void decodeScan(const ScanMsg& msg);
+  virtual void decodePacket(const PacketMsg& msg);
+};
+
+inline AdapterBase::~AdapterBase()
+{
+  stop();
+}
+
+inline void AdapterBase::start()
+{
+  return;
+}
+
+inline void AdapterBase::stop()
+{
+  return;
+}
+
+inline void AdapterBase::sendScan(const ScanMsg& msg)
+{
+  return;
+}
+
+inline void AdapterBase::sendPacket(const PacketMsg& msg)
+{
+  return;
+}
+
+inline void AdapterBase::sendPointCloud(const LidarPointCloudMsg& msg)
+{
+  return;
+}
+
+inline void AdapterBase::sendCameraTrigger(const CameraTrigger& msg)
+{
+  return;
+}
+
+inline void AdapterBase::regRecvCallback(const std::function<void(const ScanMsg&)>& callback)
+{
+  return;
+}
+
+inline void AdapterBase::regRecvCallback(const std::function<void(const PacketMsg&)>& callback)
+{
+  return;
+}
+
+inline void AdapterBase::regRecvCallback(const std::function<void(const LidarPointCloudMsg&)>& callback)
+{
+  return;
+}
+
+inline void AdapterBase::regRecvCallback(const std::function<void(const CameraTrigger&)>& callback)
+{
+  return;
+}
+
+inline void AdapterBase::decodeScan(const ScanMsg& msg)
+{
+  return;
+}
+
+inline void AdapterBase::decodePacket(const PacketMsg& msg)
+{
+  return;
+}
+
+}  // namespace lidar
+}  // namespace robosense

src/ros/open_source_code/rslidar_sdk/src/adapter/camera_trigger_adapter.hpp

@@ -0,0 +1,96 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#ifdef ROS_FOUND
+#include "adapter/adapter_base.hpp"
+#include "msg/ros_msg_translator.h"
+#include <ros/ros.h>
+#include <ros/publisher.h>
+
+namespace robosense
+{
+namespace lidar
+{
+class CameraTriggerRosAdapter : virtual public AdapterBase
+{
+public:
+  CameraTriggerRosAdapter() = default;
+  ~CameraTriggerRosAdapter() = default;
+  void init(const YAML::Node& config);
+  void sendCameraTrigger(const CameraTrigger& msg);
+
+private:
+  std::shared_ptr<ros::NodeHandle> nh_;
+  std::map<std::string, ros::Publisher> pub_map_;
+};
+
+inline void CameraTriggerRosAdapter::init(const YAML::Node& config)
+{
+  nh_ = std::unique_ptr<ros::NodeHandle>(new ros::NodeHandle());
+  if (config["camera"] && config["camera"].Type() != YAML::NodeType::Null)
+  {
+    for (auto iter : config["camera"])
+    {
+      std::string frame_id;
+      yamlRead<std::string>(iter, "frame_id", frame_id, "rs_camera");
+      ros::Publisher pub = nh_->advertise<std_msgs::Time>(frame_id + "_trigger", 10);
+      pub_map_.emplace(frame_id, pub);
+    }
+  }
+}
+
+inline void CameraTriggerRosAdapter::sendCameraTrigger(const CameraTrigger& msg)
+{
+  auto iter = pub_map_.find(msg.first);
+  if (iter != pub_map_.end())
+  {
+    iter->second.publish(toRosMsg(msg));
+  }
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS_FOUND
+
+#ifdef ROS2_FOUND
+#include "adapter/adapter_base.hpp"
+#include "msg/ros_msg_translator.h"
+#include "rclcpp/rclcpp.hpp"
+namespace robosense
+{
+namespace lidar
+{
+///< Add in the future
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS2_FOUND

src/ros/open_source_code/rslidar_sdk/src/adapter/driver_adapter.hpp

@@ -0,0 +1,267 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#include "adapter/adapter_base.hpp"
+#include "rs_driver/api/lidar_driver.h"
+
+namespace robosense
+{
+namespace lidar
+{
+class DriverAdapter : virtual public AdapterBase
+{
+public:
+  DriverAdapter();
+  ~DriverAdapter();
+  void init(const YAML::Node& config);
+  void start();
+  void stop();
+  inline void regRecvCallback(const std::function<void(const LidarPointCloudMsg&)>& callback);
+  inline void regRecvCallback(const std::function<void(const ScanMsg&)>& callback);
+  inline void regRecvCallback(const std::function<void(const PacketMsg&)>& callback);
+  inline void regRecvCallback(const std::function<void(const CameraTrigger&)>& callback);
+  void decodeScan(const ScanMsg& msg);
+  void decodePacket(const PacketMsg& msg);
+
+private:
+  void localPointsCallback(const PointCloudMsg<PointT>& msg);
+  void localScanCallback(const ScanMsg& msg);
+  void localPacketCallback(const PacketMsg& msg);
+  void localCameraTriggerCallback(const CameraTrigger& msg);
+  void localExceptionCallback(const lidar::Error& msg);
+  LidarPointCloudMsg core2SDK(const lidar::PointCloudMsg<PointT>& msg);
+
+private:
+  std::shared_ptr<lidar::LidarDriver<PointT>> driver_ptr_;
+  std::vector<std::function<void(const LidarPointCloudMsg&)>> point_cloud_cb_vec_;
+  std::vector<std::function<void(const ScanMsg&)>> scan_cb_vec_;
+  std::vector<std::function<void(const PacketMsg&)>> packet_cb_vec_;
+  std::vector<std::function<void(const CameraTrigger&)>> camera_trigger_cb_vec_;
+  lidar::ThreadPool::Ptr thread_pool_ptr_;
+};
+
+inline DriverAdapter::DriverAdapter()
+{
+  driver_ptr_.reset(new lidar::LidarDriver<PointT>());
+  thread_pool_ptr_.reset(new lidar::ThreadPool());
+  driver_ptr_->regExceptionCallback(std::bind(&DriverAdapter::localExceptionCallback, this, std::placeholders::_1));
+}
+
+inline DriverAdapter::~DriverAdapter()
+{
+  driver_ptr_->stop();
+}
+
+inline void DriverAdapter::init(const YAML::Node& config)
+{
+  lidar::RSDriverParam driver_param;
+  int msg_source;
+  std::string lidar_type;
+  uint16_t split_frame_mode;
+  YAML::Node driver_config = yamlSubNodeAbort(config, "driver");
+  yamlReadAbort<int>(config, "msg_source", msg_source);
+  yamlRead<std::string>(driver_config, "frame_id", driver_param.frame_id, "rslidar");
+  yamlRead<std::string>(driver_config, "angle_path", driver_param.angle_path, "");
+  yamlReadAbort<std::string>(driver_config, "lidar_type", lidar_type);
+  yamlRead<bool>(driver_config, "wait_for_difop", driver_param.wait_for_difop, true);
+  yamlRead<bool>(driver_config, "saved_by_rows", driver_param.saved_by_rows, false);
+  yamlRead<bool>(driver_config, "use_lidar_clock", driver_param.decoder_param.use_lidar_clock, false);
+  yamlRead<float>(driver_config, "min_distance", driver_param.decoder_param.min_distance, 0.2);
+  yamlRead<float>(driver_config, "max_distance", driver_param.decoder_param.max_distance, 200);
+  yamlRead<float>(driver_config, "start_angle", driver_param.decoder_param.start_angle, 0);
+  yamlRead<float>(driver_config, "end_angle", driver_param.decoder_param.end_angle, 360);
+  yamlRead<uint16_t>(driver_config, "split_frame_mode", split_frame_mode, 1);
+  yamlRead<uint32_t>(driver_config, "num_pkts_split", driver_param.decoder_param.num_pkts_split, 0);
+  yamlRead<float>(driver_config, "cut_angle", driver_param.decoder_param.cut_angle, 0);
+  yamlRead<std::string>(driver_config, "device_ip", driver_param.input_param.device_ip, "192.168.1.200");
+  yamlRead<std::string>(driver_config, "multi_cast_address", driver_param.input_param.multi_cast_address, "0.0.0.0");
+  yamlRead<uint16_t>(driver_config, "msop_port", driver_param.input_param.msop_port, 6699);
+  yamlRead<uint16_t>(driver_config, "difop_port", driver_param.input_param.difop_port, 7788);
+  yamlRead<bool>(driver_config, "read_pcap", driver_param.input_param.read_pcap, false);
+  yamlRead<double>(driver_config, "pcap_rate", driver_param.input_param.pcap_rate, 1);
+  yamlRead<bool>(driver_config, "pcap_repeat", driver_param.input_param.pcap_repeat, false);
+  yamlRead<std::string>(driver_config, "pcap_path", driver_param.input_param.pcap_path, "");
+  yamlRead<float>(driver_config, "x", driver_param.decoder_param.transform_param.x, 0);
+  yamlRead<float>(driver_config, "y", driver_param.decoder_param.transform_param.y, 0);
+  yamlRead<float>(driver_config, "z", driver_param.decoder_param.transform_param.z, 0);
+  yamlRead<float>(driver_config, "roll", driver_param.decoder_param.transform_param.roll, 0);
+  yamlRead<float>(driver_config, "pitch", driver_param.decoder_param.transform_param.pitch, 0);
+  yamlRead<float>(driver_config, "yaw", driver_param.decoder_param.transform_param.yaw, 0);
+  driver_param.lidar_type = driver_param.strToLidarType(lidar_type);
+  driver_param.decoder_param.split_frame_mode = SplitFrameMode(split_frame_mode);
+  if (config["camera"] && config["camera"].Type() != YAML::NodeType::Null)
+  {
+    for (size_t i = 0; i < config["camera"].size(); i++)
+    {
+      double trigger_angle;
+      std::string frame_id;
+      yamlRead<double>(config["camera"][i], "trigger_angle", trigger_angle, 0);
+      yamlRead<std::string>(config["camera"][i], "frame_id", frame_id, "rs_camera");
+      auto iter = driver_param.decoder_param.trigger_param.trigger_map.find(trigger_angle);
+      if (iter != driver_param.decoder_param.trigger_param.trigger_map.end())
+      {
+        trigger_angle += (double)i / 1000.0;
+        driver_param.decoder_param.trigger_param.trigger_map.emplace(trigger_angle, frame_id);
+      }
+      else
+      {
+        driver_param.decoder_param.trigger_param.trigger_map.emplace(trigger_angle, frame_id);
+      }
+    }
+  }
+  if (msg_source == MsgSource::MSG_FROM_LIDAR || msg_source == MsgSource::MSG_FROM_PCAP)
+  {
+    if (!driver_ptr_->init(driver_param))
+    {
+      RS_ERROR << "Driver Initialize Error...." << RS_REND;
+      exit(-1);
+    }
+  }
+  else
+  {
+    driver_ptr_->initDecoderOnly(driver_param);
+  }
+  driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localPointsCallback, this, std::placeholders::_1));
+  driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localScanCallback, this, std::placeholders::_1));
+  driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localPacketCallback, this, std::placeholders::_1));
+  driver_ptr_->regRecvCallback(std::bind(&DriverAdapter::localCameraTriggerCallback, this, std::placeholders::_1));
+}
+
+inline void DriverAdapter::start()
+{
+  driver_ptr_->start();
+}
+
+inline void DriverAdapter::stop()
+{
+  driver_ptr_->stop();
+}
+
+inline void DriverAdapter::regRecvCallback(const std::function<void(const LidarPointCloudMsg&)>& callback)
+{
+  point_cloud_cb_vec_.emplace_back(callback);
+}
+
+inline void DriverAdapter::regRecvCallback(const std::function<void(const ScanMsg&)>& callback)
+{
+  scan_cb_vec_.emplace_back(callback);
+}
+
+inline void DriverAdapter::regRecvCallback(const std::function<void(const PacketMsg&)>& callback)
+{
+  packet_cb_vec_.emplace_back(callback);
+}
+
+inline void DriverAdapter::regRecvCallback(const std::function<void(const CameraTrigger&)>& callback)
+{
+  camera_trigger_cb_vec_.emplace_back(callback);
+}
+
+inline void DriverAdapter::decodeScan(const ScanMsg& msg)
+{
+  lidar::PointCloudMsg<PointT> point_cloud_msg;
+  if (driver_ptr_->decodeMsopScan(msg, point_cloud_msg))
+  {
+    localPointsCallback(point_cloud_msg);
+  }
+}
+
+inline void DriverAdapter::decodePacket(const PacketMsg& msg)
+{
+  driver_ptr_->decodeDifopPkt(msg);
+}
+
+inline void DriverAdapter::localPointsCallback(const PointCloudMsg<PointT>& msg)
+{
+  for (auto iter : point_cloud_cb_vec_)
+  {
+    thread_pool_ptr_->commit([this, msg, iter]() { iter(core2SDK(msg)); });
+  }
+}
+
+inline void DriverAdapter::localScanCallback(const ScanMsg& msg)
+{
+  for (auto iter : scan_cb_vec_)
+  {
+    thread_pool_ptr_->commit([this, msg, iter]() { iter(msg); });
+  }
+}
+
+inline void DriverAdapter::localPacketCallback(const PacketMsg& msg)
+{
+  for (auto iter : packet_cb_vec_)
+  {
+    thread_pool_ptr_->commit([this, msg, iter]() { iter(msg); });
+  }
+}
+
+inline void DriverAdapter::localCameraTriggerCallback(const CameraTrigger& msg)
+{
+  for (auto iter : camera_trigger_cb_vec_)
+  {
+    thread_pool_ptr_->commit([this, msg, iter]() { iter(msg); });
+  }
+}
+
+inline void DriverAdapter::localExceptionCallback(const lidar::Error& msg)
+{
+  switch (msg.error_code_type)
+  {
+    case lidar::ErrCodeType::INFO_CODE:
+      RS_INFO << msg.toString() << RS_REND;
+      break;
+    case lidar::ErrCodeType::WARNING_CODE:
+      RS_WARNING << msg.toString() << RS_REND;
+      break;
+    case lidar::ErrCodeType::ERROR_CODE:
+      RS_ERROR << msg.toString() << RS_REND;
+      break;
+  }
+}
+
+inline LidarPointCloudMsg DriverAdapter::core2SDK(const lidar::PointCloudMsg<PointT>& msg)
+{
+  LidarPointCloudMsg::PointCloudPtr point_cloud(new LidarPointCloudMsg::PointCloud);
+  point_cloud->points.assign(msg.point_cloud_ptr->begin(), msg.point_cloud_ptr->end());
+  point_cloud->height = msg.height;
+  point_cloud->width = msg.width;
+  point_cloud->is_dense = msg.is_dense;
+  LidarPointCloudMsg point_cloud_msg(point_cloud);
+  point_cloud_msg.frame_id = msg.frame_id;
+  point_cloud_msg.timestamp = msg.timestamp;
+  point_cloud_msg.seq = msg.seq;
+  return std::move(point_cloud_msg);
+}
+
+}  // namespace lidar
+}  // namespace robosense

src/ros/open_source_code/rslidar_sdk/src/adapter/packet_protobuf_adapter.hpp

@@ -0,0 +1,325 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#ifdef PROTO_FOUND
+#include "adapter/adapter_base.hpp"
+#include "utility/protobuf_communicator.hpp"
+#include "msg/proto_msg_translator.h"
+constexpr size_t PKT_RECEIVE_BUF_SIZE = 2000000;
+
+namespace robosense
+{
+namespace lidar
+{
+class PacketProtoAdapter : virtual public AdapterBase
+{
+public:
+  PacketProtoAdapter();
+  ~PacketProtoAdapter();
+  void init(const YAML::Node& config);
+  void start();
+  void stop();
+  void regRecvCallback(const std::function<void(const ScanMsg&)>& callback);
+  void regRecvCallback(const std::function<void(const PacketMsg&)>& callback);
+  void sendScan(const ScanMsg& msg);
+  void sendPacket(const PacketMsg& msg);
+
+private:
+  void localMsopCallback(const ScanMsg& msg);
+  void recvMsopPkts();
+  void spliceMsopPkts();
+  void sendMsop();
+  void localDifopCallback(const PacketMsg& msg);
+  void recvDifopPkts();
+  void spliceDifopPkts();
+  void sendDifop();
+
+private:
+  std::vector<std::function<void(const ScanMsg&)>> scan_cb_vec_;
+  std::vector<std::function<void(const PacketMsg&)>> packet_cb_vec_;
+  std::unique_ptr<ProtoCommunicator> scan_proto_com_ptr_;
+  std::unique_ptr<ProtoCommunicator> packet_proto_com_ptr_;
+  lidar::ThreadPool::Ptr thread_pool_ptr_;
+  lidar::Queue<ScanMsg> scan_send_queue_;
+  lidar::Queue<PacketMsg> packet_send_queue_;
+  lidar::Queue<std::pair<void*, ProtoMsgHeader>> scan_recv_queue_;
+  lidar::Queue<std::pair<void*, ProtoMsgHeader>> packet_recv_queue_;
+  lidar::Thread scan_recv_thread_;
+  lidar::Thread packet_recv_thread_;
+  int old_frmnum_;
+  int new_frmnum_;
+  void* scan_buff_;
+};
+
+inline PacketProtoAdapter::PacketProtoAdapter() : old_frmnum_(0), new_frmnum_(0)
+{
+  thread_pool_ptr_.reset(new ThreadPool());
+}
+
+inline PacketProtoAdapter::~PacketProtoAdapter()
+{
+  stop();
+}
+
+inline void PacketProtoAdapter::init(const YAML::Node& config)
+{
+  bool send_packet_proto;
+  int msg_source = 0;
+  std::string packet_send_ip;
+  std::string msop_send_port;
+  std::string difop_send_port;
+  uint16_t msop_recv_port;
+  uint16_t difop_recv_port;
+  yamlReadAbort<int>(config, "msg_source", msg_source);
+  yamlRead<bool>(config, "send_packet_proto", send_packet_proto, false);
+  yamlReadAbort<std::string>(config["proto"], "packet_send_ip", packet_send_ip);
+  yamlReadAbort<std::string>(config["proto"], "msop_send_port", msop_send_port);
+  yamlReadAbort<std::string>(config["proto"], "difop_send_port", difop_send_port);
+  yamlReadAbort<uint16_t>(config["proto"], "msop_recv_port", msop_recv_port);
+  yamlReadAbort<uint16_t>(config["proto"], "difop_recv_port", difop_recv_port);
+  scan_proto_com_ptr_.reset(new ProtoCommunicator);
+  packet_proto_com_ptr_.reset(new ProtoCommunicator);
+  if (msg_source == MsgSource::MSG_FROM_PROTO_PACKET)
+  {
+    if ((scan_proto_com_ptr_->initReceiver(msop_recv_port) == -1) ||
+        (packet_proto_com_ptr_->initReceiver(difop_recv_port) == -1))
+    {
+      RS_ERROR << "LidarPacketsReceiver: Create UDP Receiver Socket failed or Bind Network failed!" << RS_REND;
+      exit(-1);
+    }
+    send_packet_proto = false;
+  }
+  if (send_packet_proto)
+  {
+    if ((scan_proto_com_ptr_->initSender(msop_send_port, packet_send_ip) == -1) ||
+        (packet_proto_com_ptr_->initSender(difop_send_port, packet_send_ip) == -1))
+    {
+      RS_ERROR << "LidarPacketsReceiver: Create UDP Sender Socket failed ! " << RS_REND;
+      exit(-1);
+    }
+  }
+}
+
+inline void PacketProtoAdapter::start()
+{
+  scan_buff_ = malloc(PKT_RECEIVE_BUF_SIZE);
+  scan_recv_thread_.start_ = true;
+  scan_recv_thread_.thread_.reset(new std::thread([this]() { recvMsopPkts(); }));
+  packet_recv_thread_.start_ = true;
+  packet_recv_thread_.thread_.reset(new std::thread([this]() { recvDifopPkts(); }));
+}
+
+inline void PacketProtoAdapter::stop()
+{
+  if (scan_recv_thread_.start_.load())
+  {
+    scan_recv_thread_.start_.store(false);
+    scan_recv_thread_.thread_->join();
+    free(scan_buff_);
+  }
+  if (packet_recv_thread_.start_.load())
+  {
+    packet_recv_thread_.start_.store(false);
+    packet_recv_thread_.thread_->join();
+  }
+}
+
+inline void PacketProtoAdapter::regRecvCallback(const std::function<void(const ScanMsg&)>& callback)
+{
+  scan_cb_vec_.emplace_back(callback);
+}
+
+inline void PacketProtoAdapter::regRecvCallback(const std::function<void(const PacketMsg&)>& callback)
+{
+  packet_cb_vec_.emplace_back(callback);
+}
+
+inline void PacketProtoAdapter::sendScan(const ScanMsg& msg)
+{
+  scan_send_queue_.push(msg);
+  if (scan_send_queue_.is_task_finished_.load())
+  {
+    scan_send_queue_.is_task_finished_.store(false);
+    thread_pool_ptr_->commit([this]() { sendMsop(); });
+  }
+}
+
+inline void PacketProtoAdapter::sendPacket(const PacketMsg& msg)
+{
+  packet_send_queue_.push(msg);
+  if (packet_send_queue_.is_task_finished_.load())
+  {
+    packet_send_queue_.is_task_finished_.store(false);
+    thread_pool_ptr_->commit([this]() { sendDifop(); });
+  }
+}
+
+inline void PacketProtoAdapter::localMsopCallback(const ScanMsg& msg)
+{
+  for (auto& cb : scan_cb_vec_)
+  {
+    cb(msg);
+  }
+}
+
+inline void PacketProtoAdapter::recvMsopPkts()
+{
+  bool start_check = true;
+  while (scan_recv_thread_.start_.load())
+  {
+    void* p_data = malloc(MAX_RECEIVE_LENGTH);
+    ProtoMsgHeader tmp_header;
+    int ret = scan_proto_com_ptr_->receiveProtoMsg(p_data, MAX_RECEIVE_LENGTH, tmp_header);
+    if (start_check)
+    {
+      if (tmp_header.msg_id == 0)
+      {
+        start_check = false;
+      }
+      else
+      {
+        continue;
+      }
+    }
+    if (ret == -1)
+    {
+      RS_WARNING << "Packets Protobuf receiving error" << RS_REND;
+      continue;
+    }
+    scan_recv_queue_.push(std::make_pair(p_data, tmp_header));
+    if (scan_recv_queue_.is_task_finished_.load())
+    {
+      scan_recv_queue_.is_task_finished_.store(false);
+      thread_pool_ptr_->commit([&]() { spliceMsopPkts(); });
+    }
+  }
+}
+
+inline void PacketProtoAdapter::spliceMsopPkts()
+{
+  while (scan_recv_queue_.size() > 0)
+  {
+    if (scan_recv_thread_.start_.load())
+    {
+      auto pair = scan_recv_queue_.front();
+      old_frmnum_ = new_frmnum_;
+      new_frmnum_ = pair.second.frame_num;
+      memcpy((uint8_t*)scan_buff_ + pair.second.msg_id * SPLIT_SIZE, pair.first, SPLIT_SIZE);
+      if ((old_frmnum_ == new_frmnum_) && (pair.second.msg_id == pair.second.total_msg_cnt - 1))
+      {
+        proto_msg::LidarScan proto_msg;
+        proto_msg.ParseFromArray(scan_buff_, pair.second.total_msg_length);
+        localMsopCallback(toRsMsg(proto_msg));
+      }
+    }
+    free(scan_recv_queue_.front().first);
+    scan_recv_queue_.pop();
+  }
+  scan_recv_queue_.is_task_finished_.store(true);
+}
+
+inline void PacketProtoAdapter::sendMsop()
+{
+  while (scan_send_queue_.size() > 0)
+  {
+    proto_msg::LidarScan proto_msg = toProtoMsg(scan_send_queue_.popFront());
+    if (!scan_proto_com_ptr_->sendSplitMsg<proto_msg::LidarScan>(proto_msg))
+    {
+      RS_WARNING << "Msop packets Protobuf sending error" << RS_REND;
+    }
+  }
+  scan_send_queue_.is_task_finished_.store(true);
+}
+
+inline void PacketProtoAdapter::localDifopCallback(const PacketMsg& msg)
+{
+  for (auto& cb : packet_cb_vec_)
+  {
+    cb(msg);
+  }
+}
+
+inline void PacketProtoAdapter::recvDifopPkts()
+{
+  while (packet_recv_thread_.start_.load())
+  {
+    void* p_data = malloc(MAX_RECEIVE_LENGTH);
+    ProtoMsgHeader tmp_header;
+    int ret = packet_proto_com_ptr_->receiveProtoMsg(p_data, MAX_RECEIVE_LENGTH, tmp_header);
+
+    if (ret == -1)
+    {
+      continue;
+    }
+    packet_recv_queue_.push(std::make_pair(p_data, tmp_header));
+    if (packet_recv_queue_.is_task_finished_.load())
+    {
+      packet_recv_queue_.is_task_finished_.store(false);
+      thread_pool_ptr_->commit([&]() { spliceDifopPkts(); });
+    }
+  }
+}
+
+inline void PacketProtoAdapter::spliceDifopPkts()
+{
+  while (packet_recv_queue_.size() > 0)
+  {
+    if (packet_recv_thread_.start_.load())
+    {
+      auto pair = packet_recv_queue_.front();
+      proto_msg::LidarPacket protomsg;
+      protomsg.ParseFromArray(pair.first, pair.second.msg_length);
+      localDifopCallback(toRsMsg(protomsg));
+    }
+    free(packet_recv_queue_.front().first);
+    packet_recv_queue_.pop();
+  }
+  packet_recv_queue_.is_task_finished_.store(true);
+}
+
+inline void PacketProtoAdapter::sendDifop()
+{
+  while (packet_send_queue_.size() > 0)
+  {
+    proto_msg::LidarPacket proto_msg = toProtoMsg(packet_send_queue_.popFront());
+    if (!packet_proto_com_ptr_->sendSingleMsg<proto_msg::LidarPacket>(proto_msg))
+    {
+      RS_WARNING << "Difop packets Protobuf sending error" << RS_REND;
+    }
+  }
+  packet_send_queue_.is_task_finished_.store(true);
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // PROTO_FOUND

src/ros/open_source_code/rslidar_sdk/src/adapter/packet_ros_adapter.hpp

@@ -0,0 +1,247 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#ifdef ROS_FOUND
+#include <ros/ros.h>
+#include "adapter/adapter_base.hpp"
+#include "msg/ros_msg_translator.h"
+
+namespace robosense
+{
+namespace lidar
+{
+class PacketRosAdapter : virtual public AdapterBase
+{
+public:
+  PacketRosAdapter() = default;
+  ~PacketRosAdapter() = default;
+  void init(const YAML::Node& config);
+  void regRecvCallback(const std::function<void(const ScanMsg&)>& callback);
+  void regRecvCallback(const std::function<void(const PacketMsg&)>& callback);
+  void sendScan(const ScanMsg& msg);
+  void sendPacket(const PacketMsg& msg);
+
+private:
+  void localMsopCallback(const rslidar_msgs::rslidarScan& msg);
+  void localDifopCallback(const rslidar_msgs::rslidarPacket& msg);
+
+private:
+  std::unique_ptr<ros::NodeHandle> nh_;
+  std::vector<std::function<void(const ScanMsg&)>> scan_cb_vec_;
+  std::vector<std::function<void(const PacketMsg&)>> packet_cb_vec_;
+  ros::Publisher scan_pub_;
+  ros::Publisher packet_pub_;
+  ros::Subscriber scan_sub_;
+  ros::Subscriber packet_sub_;
+  LidarType lidar_type_;
+};
+
+inline void PacketRosAdapter::init(const YAML::Node& config)
+{
+  int msg_source;
+  bool send_packet_ros;
+  std::string ros_recv_topic;
+  std::string ros_send_topic;
+  std::string lidar_type_str;
+  nh_ = std::unique_ptr<ros::NodeHandle>(new ros::NodeHandle());
+  yamlReadAbort<int>(config, "msg_source", msg_source);
+  yamlRead<bool>(config, "send_packet_ros", send_packet_ros, false);
+  yamlRead<std::string>(config["driver"], "lidar_type", lidar_type_str, "RS16");
+  yamlRead<std::string>(config["ros"], "ros_recv_packet_topic", ros_recv_topic, "rslidar_packets");
+  yamlRead<std::string>(config["ros"], "ros_send_packet_topic", ros_send_topic, "rslidar_packets");
+  lidar_type_ = RSDriverParam::strToLidarType(lidar_type_str);
+  if (msg_source == MsgSource::MSG_FROM_ROS_PACKET)
+  {
+    packet_sub_ = nh_->subscribe(ros_recv_topic + "_difop", 1, &PacketRosAdapter::localDifopCallback, this);
+    scan_sub_ = nh_->subscribe(ros_recv_topic, 1, &PacketRosAdapter::localMsopCallback, this);
+    send_packet_ros = false;
+  }
+  if (send_packet_ros)
+  {
+    packet_pub_ = nh_->advertise<rslidar_msgs::rslidarPacket>(ros_send_topic + "_difop", 10);
+    scan_pub_ = nh_->advertise<rslidar_msgs::rslidarScan>(ros_send_topic, 10);
+  }
+}
+
+inline void PacketRosAdapter::regRecvCallback(const std::function<void(const ScanMsg&)>& callback)
+{
+  scan_cb_vec_.emplace_back(callback);
+}
+
+inline void PacketRosAdapter::regRecvCallback(const std::function<void(const PacketMsg&)>& callback)
+{
+  packet_cb_vec_.emplace_back(callback);
+}
+
+inline void PacketRosAdapter::sendScan(const ScanMsg& msg)
+{
+  scan_pub_.publish(toRosMsg(msg));
+}
+
+inline void PacketRosAdapter::sendPacket(const PacketMsg& msg)
+{
+  packet_pub_.publish(toRosMsg(msg));
+}
+
+inline void PacketRosAdapter::localMsopCallback(const rslidar_msgs::rslidarScan& msg)
+{
+  for (auto& cb : scan_cb_vec_)
+  {
+    cb(toRsMsg(lidar_type_, PktType::MSOP, msg));
+  }
+}
+
+inline void PacketRosAdapter::localDifopCallback(const rslidar_msgs::rslidarPacket& msg)
+{
+  for (auto& cb : packet_cb_vec_)
+  {
+    cb(toRsMsg(lidar_type_, PktType::DIFOP, msg));
+  }
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS_FOUND
+
+#ifdef ROS2_FOUND
+#include <rclcpp/rclcpp.hpp>
+#include "adapter/adapter_base.hpp"
+#include "msg/ros_msg_translator.h"
+namespace robosense
+{
+namespace lidar
+{
+class PacketRosAdapter : virtual public AdapterBase
+{
+public:
+  PacketRosAdapter() = default;
+  ~PacketRosAdapter();
+  void init(const YAML::Node& config);
+  void start();
+  void regRecvCallback(const std::function<void(const ScanMsg&)>& callback);
+  void regRecvCallback(const std::function<void(const PacketMsg&)>& callback);
+  void sendScan(const ScanMsg& msg);
+  void sendPacket(const PacketMsg& msg);
+
+private:
+  void localMsopCallback(const rslidar_msg::msg::RslidarScan::SharedPtr msg);
+  void localDifopCallback(const rslidar_msg::msg::RslidarPacket::SharedPtr msg);
+
+private:
+  std::shared_ptr<rclcpp::Node> node_ptr_;
+  rclcpp::Publisher<rslidar_msg::msg::RslidarScan>::SharedPtr scan_pub_;
+  rclcpp::Publisher<rslidar_msg::msg::RslidarPacket>::SharedPtr packet_pub_;
+  rclcpp::Subscription<rslidar_msg::msg::RslidarScan>::SharedPtr scan_sub_;
+  rclcpp::Subscription<rslidar_msg::msg::RslidarPacket>::SharedPtr packet_sub_;
+  std::vector<std::function<void(const ScanMsg&)>> scan_cb_vec_;
+  std::vector<std::function<void(const PacketMsg&)>> packet_cb_vec_;
+  LidarType lidar_type_;
+};
+inline PacketRosAdapter::~PacketRosAdapter()
+{
+  stop();
+}
+
+inline void PacketRosAdapter::init(const YAML::Node& config)
+{
+  int msg_source;
+  bool send_packet_ros;
+  std::string ros_recv_topic;
+  std::string ros_send_topic;
+  std::string lidar_type_str;
+  node_ptr_.reset(new rclcpp::Node("rslidar_packets_adapter"));
+  yamlReadAbort<int>(config, "msg_source", msg_source);
+  yamlRead<bool>(config, "send_packet_ros", send_packet_ros, false);
+  yamlRead<std::string>(config["driver"], "lidar_type", lidar_type_str, "RS16");
+  yamlRead<std::string>(config["ros"], "ros_recv_packet_topic", ros_recv_topic, "rslidar_packets");
+  yamlRead<std::string>(config["ros"], "ros_send_packet_topic", ros_send_topic, "rslidar_packets");
+  lidar_type_ = RSDriverParam::strToLidarType(lidar_type_str);
+  if (msg_source == MsgSource::MSG_FROM_ROS_PACKET)
+  {
+    packet_sub_ = node_ptr_->create_subscription<rslidar_msg::msg::RslidarPacket>(
+        ros_recv_topic + "_difop", 10,
+        [this](const rslidar_msg::msg::RslidarPacket::SharedPtr msg) { localDifopCallback(msg); });
+    scan_sub_ = node_ptr_->create_subscription<rslidar_msg::msg::RslidarScan>(
+        ros_recv_topic, 10, [this](const rslidar_msg::msg::RslidarScan::SharedPtr msg) { localMsopCallback(msg); });
+  }
+  if (send_packet_ros)
+  {
+    packet_pub_ = node_ptr_->create_publisher<rslidar_msg::msg::RslidarPacket>(ros_send_topic + "_difop", 10);
+    scan_pub_ = node_ptr_->create_publisher<rslidar_msg::msg::RslidarScan>(ros_send_topic, 10);
+  }
+}
+
+inline void PacketRosAdapter::start()
+{
+  std::thread t([this]() { rclcpp::spin(node_ptr_); });
+  t.detach();
+}
+
+inline void PacketRosAdapter::regRecvCallback(const std::function<void(const ScanMsg&)>& callback)
+{
+  scan_cb_vec_.emplace_back(callback);
+}
+
+inline void PacketRosAdapter::regRecvCallback(const std::function<void(const PacketMsg&)>& callback)
+{
+  packet_cb_vec_.emplace_back(callback);
+}
+
+inline void PacketRosAdapter::sendScan(const ScanMsg& msg)
+{
+  scan_pub_->publish(toRosMsg(msg));
+}
+
+inline void PacketRosAdapter::sendPacket(const PacketMsg& msg)
+{
+  packet_pub_->publish(toRosMsg(msg));
+}
+
+inline void PacketRosAdapter::localMsopCallback(const rslidar_msg::msg::RslidarScan::SharedPtr msg)
+{
+  for (auto& cb : scan_cb_vec_)
+  {
+    cb(toRsMsg(lidar_type_, PktType::MSOP, *msg));
+  }
+}
+
+inline void PacketRosAdapter::localDifopCallback(const rslidar_msg::msg::RslidarPacket::SharedPtr msg)
+{
+  for (auto& cb : packet_cb_vec_)
+  {
+    cb(toRsMsg(lidar_type_, PktType::DIFOP, *msg));
+  }
+}
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS2_FOUND

src/ros/open_source_code/rslidar_sdk/src/adapter/point_cloud_protobuf_adapter.hpp

@@ -0,0 +1,233 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#ifdef PROTO_FOUND
+#include "adapter/adapter_base.hpp"
+#include "utility/protobuf_communicator.hpp"
+#include "msg/proto_msg_translator.h"
+constexpr size_t RECEIVE_BUF_SIZE = 10000000;
+
+namespace robosense
+{
+namespace lidar
+{
+class PointCloudProtoAdapter : virtual public AdapterBase
+{
+public:
+  PointCloudProtoAdapter();
+  ~PointCloudProtoAdapter();
+  void init(const YAML::Node& config);
+  void start();
+  void stop();
+  void regRecvCallback(const std::function<void(const LidarPointCloudMsg&)>& callback);
+  void sendPointCloud(const LidarPointCloudMsg& msg);
+
+private:
+  void localCallback(const LidarPointCloudMsg& msg);
+  void sendPoints();
+  void recvPoints();
+  void splicePoints();
+
+private:
+  std::vector<std::function<void(const LidarPointCloudMsg&)>> point_cloud_cb_vec_;
+  lidar::Queue<LidarPointCloudMsg> point_cloud_send_queue_;
+  lidar::Queue<std::pair<void*, ProtoMsgHeader>> point_cloud_recv_queue_;
+  std::unique_ptr<ProtoCommunicator> proto_com_ptr_;
+  lidar::ThreadPool::Ptr thread_pool_ptr_;
+  lidar::Thread recv_thread_;
+  int old_frmnum_;
+  int new_frmnum_;
+  void* buff_;
+};
+
+inline PointCloudProtoAdapter::PointCloudProtoAdapter() : old_frmnum_(0), new_frmnum_(0)
+{
+  thread_pool_ptr_.reset(new lidar::ThreadPool());
+}
+
+inline PointCloudProtoAdapter::~PointCloudProtoAdapter()
+{
+  stop();
+}
+
+inline void PointCloudProtoAdapter::init(const YAML::Node& config)
+{
+  int msg_source = 0;
+  bool send_point_cloud_proto;
+  std::string point_cloud_send_port;
+  std::string point_cloud_send_ip;
+  uint16_t point_cloud_recv_port;
+  proto_com_ptr_.reset(new ProtoCommunicator);
+  yamlReadAbort<int>(config, "msg_source", msg_source);
+  yamlRead<bool>(config, "send_point_cloud_proto", send_point_cloud_proto, false);
+  yamlReadAbort<std::string>(config["proto"], "point_cloud_send_port", point_cloud_send_port);
+  yamlReadAbort<std::string>(config["proto"], "point_cloud_send_ip", point_cloud_send_ip);
+  yamlReadAbort<uint16_t>(config["proto"], "point_cloud_recv_port", point_cloud_recv_port);
+  if (msg_source == MsgSource::MSG_FROM_PROTO_POINTCLOUD)
+  {
+    if (proto_com_ptr_->initReceiver(point_cloud_recv_port) == -1)
+    {
+      RS_ERROR << "PointCloudProtoAdapter: Create UDP Receiver Socket failed or Bind Network failed!" << RS_REND;
+      exit(-1);
+    }
+    send_point_cloud_proto = false;
+  }
+  if (send_point_cloud_proto)
+  {
+    if (proto_com_ptr_->initSender(point_cloud_send_port, point_cloud_send_ip) == -1)
+    {
+      RS_ERROR << "PointCloudProtoAdapter: Create UDP Sender Socket failed ! " << RS_REND;
+      exit(-1);
+    }
+  }
+}
+
+inline void PointCloudProtoAdapter::start()
+{
+  buff_ = malloc(RECEIVE_BUF_SIZE);
+  recv_thread_.start_ = true;
+  recv_thread_.thread_.reset(new std::thread([this]() { recvPoints(); }));
+}
+
+inline void PointCloudProtoAdapter::stop()
+{
+  if (recv_thread_.start_.load())
+  {
+    recv_thread_.start_.store(false);
+    recv_thread_.thread_->join();
+    free(buff_);
+  }
+}
+
+inline void PointCloudProtoAdapter::regRecvCallback(const std::function<void(const LidarPointCloudMsg&)>& callback)
+{
+  point_cloud_cb_vec_.emplace_back(callback);
+}
+
+inline void PointCloudProtoAdapter::sendPointCloud(const LidarPointCloudMsg& msg)
+{
+  if (point_cloud_send_queue_.size() > 10)
+  {
+    RS_WARNING << "Point Cloud Protobuf Sender buffer over flow!" << RS_REND;
+    point_cloud_send_queue_.clear();
+  }
+  point_cloud_send_queue_.push(msg);
+  if (point_cloud_send_queue_.is_task_finished_.load())
+  {
+    point_cloud_send_queue_.is_task_finished_.store(false);
+    thread_pool_ptr_->commit([this]() { sendPoints(); });
+  }
+}
+
+inline void PointCloudProtoAdapter::localCallback(const LidarPointCloudMsg& rs_msg)
+{
+  for (auto& cb : point_cloud_cb_vec_)
+  {
+    cb(rs_msg);
+  }
+}
+
+inline void PointCloudProtoAdapter::sendPoints()
+{
+  while (point_cloud_send_queue_.size() > 0)
+  {
+    proto_msg::LidarPointCloud proto_msg = toProtoMsg(point_cloud_send_queue_.popFront());
+    if (!proto_com_ptr_->sendSplitMsg<proto_msg::LidarPointCloud>(proto_msg))
+    {
+      RS_WARNING << "PointCloud Protobuf sending error" << RS_REND;
+    }
+  }
+  point_cloud_send_queue_.is_task_finished_.store(true);
+}
+
+inline void PointCloudProtoAdapter::recvPoints()
+{
+  bool start_check = true;
+  while (recv_thread_.start_.load())
+  {
+    void* p_data = malloc(MAX_RECEIVE_LENGTH);
+    ProtoMsgHeader tmp_header;
+    int ret = proto_com_ptr_->receiveProtoMsg(p_data, MAX_RECEIVE_LENGTH, tmp_header);
+    if (start_check)
+    {
+      if (tmp_header.msg_id == 0)
+      {
+        start_check = false;
+      }
+      else
+      {
+        continue;
+      }
+    }
+    if (ret == -1)
+    {
+      RS_WARNING << "PointCloud Protobuf receiving error" << RS_REND;
+      continue;
+    }
+
+    point_cloud_recv_queue_.push(std::make_pair(p_data, tmp_header));
+
+    if (point_cloud_recv_queue_.is_task_finished_.load())
+    {
+      point_cloud_recv_queue_.is_task_finished_.store(false);
+      thread_pool_ptr_->commit([&]() { splicePoints(); });
+    }
+  }
+}
+
+inline void PointCloudProtoAdapter::splicePoints()
+{
+  while (point_cloud_recv_queue_.size() > 0)
+  {
+    if (recv_thread_.start_.load())
+    {
+      auto pair = point_cloud_recv_queue_.front();
+      old_frmnum_ = new_frmnum_;
+      new_frmnum_ = pair.second.frame_num;
+      memcpy((uint8_t*)buff_ + pair.second.msg_id * SPLIT_SIZE, pair.first, SPLIT_SIZE);
+      if ((old_frmnum_ == new_frmnum_) && (pair.second.msg_id == pair.second.total_msg_cnt - 1))
+      {
+        proto_msg::LidarPointCloud proto_msg;
+        proto_msg.ParseFromArray(buff_, pair.second.total_msg_length);
+        localCallback(toRsMsg(proto_msg));
+      }
+    }
+    free(point_cloud_recv_queue_.front().first);
+    point_cloud_recv_queue_.pop();
+  }
+  point_cloud_recv_queue_.is_task_finished_.store(true);
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // PROTO_FOUND

src/ros/open_source_code/rslidar_sdk/src/adapter/point_cloud_ros_adapter.hpp

@@ -0,0 +1,119 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#ifdef ROS_FOUND
+#include <ros/ros.h>
+#include "adapter/adapter_base.hpp"
+#include "msg/ros_msg_translator.h"
+
+namespace robosense
+{
+namespace lidar
+{
+class PointCloudRosAdapter : virtual public AdapterBase
+{
+public:
+  PointCloudRosAdapter() = default;
+  ~PointCloudRosAdapter() = default;
+  void init(const YAML::Node& config);
+  void sendPointCloud(const LidarPointCloudMsg& msg);
+
+private:
+  std::shared_ptr<ros::NodeHandle> nh_;
+  ros::Publisher point_cloud_pub_;
+};
+
+inline void PointCloudRosAdapter::init(const YAML::Node& config)
+{
+  bool send_point_cloud_ros;
+  std::string ros_send_topic;
+  nh_ = std::unique_ptr<ros::NodeHandle>(new ros::NodeHandle());
+  yamlRead<bool>(config, "send_point_cloud_ros", send_point_cloud_ros, false);
+  yamlRead<std::string>(config["ros"], "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points");
+  if (send_point_cloud_ros)
+  {
+    point_cloud_pub_ = nh_->advertise<sensor_msgs::PointCloud2>(ros_send_topic, 10);
+  }
+}
+
+inline void PointCloudRosAdapter::sendPointCloud(const LidarPointCloudMsg& msg)
+{
+  point_cloud_pub_.publish(toRosMsg(msg));
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS_FOUND
+
+#ifdef ROS2_FOUND
+#include <rclcpp/rclcpp.hpp>
+#include "adapter/adapter_base.hpp"
+#include "msg/ros_msg_translator.h"
+namespace robosense
+{
+namespace lidar
+{
+class PointCloudRosAdapter : virtual public AdapterBase
+{
+public:
+  PointCloudRosAdapter() = default;
+  ~PointCloudRosAdapter() = default;
+  void init(const YAML::Node& config);
+  void sendPointCloud(const LidarPointCloudMsg& msg);
+
+private:
+  std::shared_ptr<rclcpp::Node> node_ptr_;
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr point_cloud_pub_;
+};
+
+inline void PointCloudRosAdapter::init(const YAML::Node& config)
+{
+  bool send_point_cloud_ros;
+  std::string ros_send_topic;
+  node_ptr_.reset(new rclcpp::Node("rslidar_points_adapter"));
+  yamlRead<bool>(config, "send_point_cloud_ros", send_point_cloud_ros, false);
+  yamlRead<std::string>(config["ros"], "ros_send_point_cloud_topic", ros_send_topic, "rslidar_points");
+  if (send_point_cloud_ros)
+  {
+    point_cloud_pub_ = node_ptr_->create_publisher<sensor_msgs::msg::PointCloud2>(ros_send_topic, 1);
+  }
+}
+
+inline void PointCloudRosAdapter::sendPointCloud(const LidarPointCloudMsg& msg)
+{
+  point_cloud_pub_->publish(toRosMsg(msg));
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS2_FOUND

src/ros/open_source_code/rslidar_sdk/src/manager/adapter_manager.cpp

@@ -0,0 +1,403 @@
+/*********************************************************************************************************************
+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 "manager/adapter_manager.h"
+namespace robosense
+{
+namespace lidar
+{
+AdapterManager::~AdapterManager()
+{
+  stop();
+}
+
+void AdapterManager::init(const YAML::Node& config)
+{
+  packet_thread_flag_ = false;
+  point_cloud_thread_flag_ = false;
+  int msg_source = 0;
+  bool send_point_cloud_ros;
+  bool send_packet_ros;
+  bool send_point_cloud_proto;
+  bool send_packet_proto;
+  bool send_camera_trigger_ros;
+  std::string pcap_dir;
+  double pcap_rate;
+  bool pcap_repeat;
+  YAML::Node common_config = yamlSubNodeAbort(config, "common");
+  yamlRead<int>(common_config, "msg_source", msg_source, 0);
+  yamlRead<bool>(common_config, "send_packet_ros", send_packet_ros, false);
+  yamlRead<bool>(common_config, "send_point_cloud_ros", send_point_cloud_ros, false);
+  yamlRead<bool>(common_config, "send_point_cloud_proto", send_point_cloud_proto, false);
+  yamlRead<bool>(common_config, "send_packet_proto", send_packet_proto, false);
+  yamlRead<bool>(common_config, "send_camera_trigger_ros", send_camera_trigger_ros, false);
+  yamlRead<std::string>(common_config, "pcap_path", pcap_dir, "");
+  yamlRead<double>(common_config, "pcap_rate", pcap_rate, 1);
+  yamlRead<bool>(common_config, "pcap_repeat", pcap_repeat, true);
+  YAML::Node lidar_config = yamlSubNodeAbort(config, "lidar");
+  for (uint8_t i = 0; i < lidar_config.size(); ++i)
+  {
+    AdapterBase::Ptr recv_ptr;
+    /*Receiver*/
+    switch (msg_source)
+    {
+      case MsgSource::MSG_FROM_LIDAR:  // use driver
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        RS_INFO << "Receive Packets From : Online LiDAR" << RS_REND;
+        RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as<uint16_t>() << RS_REND;
+        RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as<uint16_t>() << RS_REND;
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        point_cloud_thread_flag_ = true;
+        lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_LIDAR;
+        recv_ptr = createReceiver(lidar_config[i], AdapterType::DriverAdapter);
+        point_cloud_receive_adapter_vec_.push_back(recv_ptr);
+        if (send_packet_ros || send_packet_proto)
+        {
+          packet_receive_adapter_vec_.push_back(recv_ptr);
+          packet_thread_flag_ = true;
+        }
+        break;
+
+      case MsgSource::MSG_FROM_ROS_PACKET:  // pkt from ros
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        RS_INFO << "Receive Packets From : ROS" << RS_REND;
+        RS_INFO << "Msop Topic: " << lidar_config[i]["ros"]["ros_recv_packet_topic"].as<std::string>() << RS_REND;
+        RS_INFO << "Difop Topic: " << lidar_config[i]["ros"]["ros_recv_packet_topic"].as<std::string>() << "_difop"
+                << RS_REND;
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        point_cloud_thread_flag_ = false;
+        packet_thread_flag_ = true;
+        lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_ROS_PACKET;
+        send_packet_ros = false;
+        point_cloud_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::DriverAdapter));
+        packet_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::PacketRosAdapter));
+        packet_receive_adapter_vec_[i]->regRecvCallback(
+            std::bind(&AdapterBase::decodeScan, point_cloud_receive_adapter_vec_[i], std::placeholders::_1));
+        packet_receive_adapter_vec_[i]->regRecvCallback(
+            std::bind(&AdapterBase::decodePacket, point_cloud_receive_adapter_vec_[i], std::placeholders::_1));
+        break;
+
+      case MsgSource::MSG_FROM_PCAP:  // pcap
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        RS_INFO << "Receive Packets From : Pcap" << RS_REND;
+        RS_INFO << "Msop Port: " << lidar_config[i]["driver"]["msop_port"].as<uint16_t>() << RS_REND;
+        RS_INFO << "Difop Port: " << lidar_config[i]["driver"]["difop_port"].as<uint16_t>() << RS_REND;
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        point_cloud_thread_flag_ = true;
+        lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_PCAP;
+        lidar_config[i]["driver"]["read_pcap"] = true;
+        lidar_config[i]["driver"]["pcap_path"] = pcap_dir;
+        lidar_config[i]["driver"]["pcap_rate"] = pcap_rate;
+        lidar_config[i]["driver"]["pcap_repeat"] = pcap_repeat;
+        recv_ptr = createReceiver(lidar_config[i], AdapterType::DriverAdapter);
+        point_cloud_receive_adapter_vec_.push_back(recv_ptr);
+        if (send_packet_ros || send_packet_proto)
+        {
+          packet_receive_adapter_vec_.push_back(recv_ptr);
+          packet_thread_flag_ = true;
+        }
+        break;
+
+      case MsgSource::MSG_FROM_PROTO_PACKET:  // packets from proto
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        RS_INFO << "Receive Packets From : Protobuf-UDP" << RS_REND;
+        RS_INFO << "Msop Port: " << lidar_config[i]["proto"]["msop_recv_port"].as<uint16_t>() << RS_REND;
+        RS_INFO << "Difop Port: " << lidar_config[i]["proto"]["difop_recv_port"].as<uint16_t>() << RS_REND;
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        point_cloud_thread_flag_ = false;
+        packet_thread_flag_ = true;
+        lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_PROTO_PACKET;
+        send_packet_proto = false;
+        point_cloud_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::DriverAdapter));
+        packet_receive_adapter_vec_.emplace_back(createReceiver(lidar_config[i], AdapterType::PacketProtoAdapter));
+        packet_receive_adapter_vec_[i]->regRecvCallback(
+            std::bind(&AdapterBase::decodeScan, point_cloud_receive_adapter_vec_[i], std::placeholders::_1));
+        packet_receive_adapter_vec_[i]->regRecvCallback(
+            std::bind(&AdapterBase::decodePacket, point_cloud_receive_adapter_vec_[i], std::placeholders::_1));
+        break;
+
+      case MsgSource::MSG_FROM_PROTO_POINTCLOUD:  // point cloud from proto
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        RS_INFO << "Receive PointCloud From : Protobuf-UDP" << RS_REND;
+        RS_INFO << "PointCloud Port: " << lidar_config[i]["proto"]["point_cloud_recv_port"].as<uint16_t>() << RS_REND;
+        RS_INFO << "------------------------------------------------------" << RS_REND;
+        point_cloud_thread_flag_ = true;
+        packet_thread_flag_ = false;
+        lidar_config[i]["msg_source"] = (int)MsgSource::MSG_FROM_PROTO_POINTCLOUD;
+        send_point_cloud_proto = false;
+        send_packet_ros = false;
+        send_packet_proto = false;
+        send_camera_trigger_ros = false;
+        point_cloud_receive_adapter_vec_.emplace_back(
+            createReceiver(lidar_config[i], AdapterType::PointCloudProtoAdapter));
+        packet_receive_adapter_vec_.emplace_back(nullptr);
+        break;
+
+      default:
+        RS_ERROR << "Not use LiDAR or Wrong LiDAR message source! Abort!" << RS_REND;
+        exit(-1);
+    }
+
+    /*Transmitter*/
+    if (send_packet_ros)
+    {
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      RS_DEBUG << "Send Packets To : ROS" << RS_REND;
+      RS_DEBUG << "Msop Topic: " << lidar_config[i]["ros"]["ros_send_packet_topic"].as<std::string>() << RS_REND;
+      RS_DEBUG << "Difop Topic: " << lidar_config[i]["ros"]["ros_send_packet_topic"].as<std::string>() << "_difop"
+               << RS_REND;
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      lidar_config[i]["send_packet_ros"] = true;
+      AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PacketRosAdapter);
+      packet_transmit_adapter_vec_.emplace_back(transmitter_ptr);
+      packet_receive_adapter_vec_[i]->regRecvCallback(
+          std::bind(&AdapterBase::sendScan, transmitter_ptr, std::placeholders::_1));
+      packet_receive_adapter_vec_[i]->regRecvCallback(
+          std::bind(&AdapterBase::sendPacket, transmitter_ptr, std::placeholders::_1));
+    }
+    if (send_packet_proto)
+    {
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      RS_DEBUG << "Send Packets To : Protobuf-UDP" << RS_REND;
+      RS_DEBUG << "Msop Port:  " << lidar_config[i]["proto"]["msop_send_port"].as<uint16_t>() << RS_REND;
+      RS_DEBUG << "Difop Port: " << lidar_config[i]["proto"]["difop_send_port"].as<uint16_t>() << RS_REND;
+      RS_DEBUG << "Target IP: " << lidar_config[i]["proto"]["packet_send_ip"].as<std::string>() << RS_REND;
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      lidar_config[i]["send_packet_proto"] = true;
+      AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PacketProtoAdapter);
+      packet_transmit_adapter_vec_.emplace_back(transmitter_ptr);
+      packet_receive_adapter_vec_[i]->regRecvCallback(
+          std::bind(&AdapterBase::sendScan, transmitter_ptr, std::placeholders::_1));
+      packet_receive_adapter_vec_[i]->regRecvCallback(
+          std::bind(&AdapterBase::sendPacket, transmitter_ptr, std::placeholders::_1));
+    }
+    if (send_point_cloud_ros)
+    {
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      RS_DEBUG << "Send PointCloud To : ROS" << RS_REND;
+      RS_DEBUG << "PointCloud Topic: " << lidar_config[i]["ros"]["ros_send_point_cloud_topic"].as<std::string>()
+               << RS_REND;
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      lidar_config[i]["send_point_cloud_ros"] = true;
+      AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PointCloudRosAdapter);
+      point_cloud_transmit_adapter_vec_.emplace_back(transmitter_ptr);
+      point_cloud_receive_adapter_vec_[i]->regRecvCallback(
+          std::bind(&AdapterBase::sendPointCloud, transmitter_ptr, std::placeholders::_1));
+    }
+    if (send_point_cloud_proto)
+    {
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      RS_DEBUG << "Send PointCloud To : Protobuf-UDP" << RS_REND;
+      RS_DEBUG << "PointCloud Port:  " << lidar_config[i]["proto"]["point_cloud_send_port"].as<uint16_t>() << RS_REND;
+      RS_DEBUG << "Target IP: " << lidar_config[i]["proto"]["point_cloud_send_ip"].as<std::string>() << RS_REND;
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      lidar_config[i]["send_point_cloud_proto"] = true;
+      AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::PointCloudProtoAdapter);
+      point_cloud_transmit_adapter_vec_.emplace_back(transmitter_ptr);
+      point_cloud_receive_adapter_vec_[i]->regRecvCallback(
+          std::bind(&AdapterBase::sendPointCloud, transmitter_ptr, std::placeholders::_1));
+    }
+    if (send_camera_trigger_ros)
+    {
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      RS_DEBUG << "Send Camera Trigger To : ROS" << RS_REND;
+      for (auto iter : lidar_config[i]["camera"])
+      {
+        RS_DEBUG << "Camera : " << iter["frame_id"].as<std::string>()
+                 << "  Trigger Angle : " << iter["trigger_angle"].as<double>() << RS_REND;
+      }
+      RS_DEBUG << "------------------------------------------------------" << RS_REND;
+      AdapterBase::Ptr transmitter_ptr = createTransmitter(lidar_config[i], AdapterType::CameraTriggerRosAdapter);
+      point_cloud_receive_adapter_vec_[i]->regRecvCallback(
+          std::bind(&AdapterBase::sendCameraTrigger, transmitter_ptr, std::placeholders::_1));
+    }
+  }
+}
+
+void AdapterManager::start()
+{
+  if (point_cloud_thread_flag_)
+  {
+    for (auto& iter : point_cloud_receive_adapter_vec_)
+    {
+      if (iter != nullptr)
+        iter->start();
+    }
+  }
+  if (packet_thread_flag_)
+  {
+    for (auto& iter : packet_receive_adapter_vec_)
+    {
+      if (iter != nullptr)
+      {
+        iter->start();
+      }
+    }
+  }
+}
+
+void AdapterManager::stop()
+{
+  if (point_cloud_thread_flag_)
+  {
+    for (auto& iter : point_cloud_receive_adapter_vec_)
+    {
+      if (iter != nullptr)
+        iter->stop();
+    }
+  }
+  if (packet_thread_flag_)
+  {
+    for (auto& iter : packet_receive_adapter_vec_)
+    {
+      if (iter != nullptr)
+      {
+        iter->stop();
+      }
+    }
+  }
+}
+
+std::shared_ptr<AdapterBase> AdapterManager::createReceiver(const YAML::Node& config, const AdapterType& adapter_type)
+{
+  std::shared_ptr<AdapterBase> receiver;
+  switch (adapter_type)
+  {
+    case AdapterType::DriverAdapter:
+      receiver = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<DriverAdapter>());
+      receiver->init(config);
+      break;
+
+    case AdapterType::PacketRosAdapter:
+#if (ROS_FOUND || ROS2_FOUND)
+      receiver = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<PacketRosAdapter>());
+      receiver->init(config);
+      break;
+#else
+      RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    case AdapterType::PacketProtoAdapter:
+#ifdef PROTO_FOUND
+      receiver = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<PacketProtoAdapter>());
+      receiver->init(config);
+      break;
+#else
+      RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    case AdapterType::PointCloudProtoAdapter:
+#ifdef PROTO_FOUND
+      receiver = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<PointCloudProtoAdapter>());
+      receiver->init(config);
+      break;
+#else
+      RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    default:
+      RS_ERROR << "Create receiver failed. Abort!" << RS_REND;
+      exit(-1);
+  }
+
+  return receiver;
+}
+
+std::shared_ptr<AdapterBase> AdapterManager::createTransmitter(const YAML::Node& config,
+                                                               const AdapterType& adapter_type)
+{
+  std::shared_ptr<AdapterBase> transmitter;
+  switch (adapter_type)
+  {
+    case AdapterType::PacketRosAdapter:
+#if (ROS_FOUND || ROS2_FOUND)
+      transmitter = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<PacketRosAdapter>());
+      transmitter->init(config);
+      break;
+#else
+      RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    case AdapterType::PacketProtoAdapter:
+#ifdef PROTO_FOUND
+      transmitter = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<PacketProtoAdapter>());
+      transmitter->init(config);
+      break;
+#else
+      RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    case AdapterType::PointCloudRosAdapter:
+#if (ROS_FOUND || ROS2_FOUND)
+      transmitter = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<PointCloudRosAdapter>());
+      transmitter->init(config);
+      break;
+#else
+      RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    case AdapterType::PointCloudProtoAdapter:
+#ifdef PROTO_FOUND
+      transmitter = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<PointCloudProtoAdapter>());
+      transmitter->init(config);
+      break;
+#else
+      RS_ERROR << "Protobuf not found! Could not use Protobuf functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    case AdapterType::CameraTriggerRosAdapter:
+#if (ROS_FOUND)  ///< Camera trigger not support ROS2 yet
+      transmitter = std::dynamic_pointer_cast<AdapterBase>(std::make_shared<CameraTriggerRosAdapter>());
+      transmitter->init(config);
+      break;
+#else
+      RS_ERROR << "ROS not found! Could not use ROS functions!" << RS_REND;
+      exit(-1);
+#endif
+
+    default:
+      RS_ERROR << "Create transmitter failed. Abort!" << RS_REND;
+      exit(-1);
+  }
+
+  return transmitter;
+}
+
+}  // namespace lidar
+}  // namespace robosense

src/ros/open_source_code/rslidar_sdk/src/manager/adapter_manager.h

@@ -0,0 +1,107 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+/********************************************************************************************************************
+ * Basic struct:
+ *
+ *
+ *                                              AdapterBase
+ *                                            /      |      \
+ *                                           /       |       \
+ *     ROS:                        PacketRosAdapter  |   PointCloudRosAdapter
+ *     Protobuf:          PacketProtoAdapter         |            PointCloudProtoAdapter
+ *     Driver:                       \         DriverAdapter           /
+ *                                    \        /           \          /
+ *                                     \      /             \        /
+ *                                      \    /               \      /
+ *                                      Packet              PointCloud
+ *
+ *
+ * AdapterManager:
+ *
+ * step1
+ *                  -msg_source=1 -> packet receiver: DriverAdapter; point cloud receiver: DriverAdapter
+ *                  -msg_source=2 -> packet receiver: PacketRosAdapter; point cloud receiver: DriverAdapter
+ * createReceiver ->-msg_source=3 -> packet receiver: DriverAdapter; point cloud receiver: DriverAdapter
+ *                  -msg_source=4 -> packet receiver: PacketProtoAdapter; point cloud receiver: DriverAdapter
+ *                  -msg_source=5 -> packet receiver: None; point cloud receiver: PointCloudProtoAdapter
+ *
+ * step2
+ *
+ *                      -send_packet_ros=true -> PacketRosAdapter
+ * createTransmitter -> -send_point_cloud_ros=true -> PointCloudRosAdapter
+ *                      -send_packet_proto=true -> PacketProtoAdapter
+ *                      -send_point_cloud_proto=true -> PointCloudProtoAdapter
+ *
+ * step3
+ * Register the transmitter's sending functions into the receiver
+ *
+ * step4
+ * Start all the receivers
+ *
+ * ******************************************************************************************************************/
+
+#pragma once
+#include "utility/yaml_reader.hpp"
+#include "adapter/driver_adapter.hpp"
+#include "adapter/packet_ros_adapter.hpp"
+#include "adapter/point_cloud_ros_adapter.hpp"
+#include "adapter/packet_protobuf_adapter.hpp"
+#include "adapter/point_cloud_protobuf_adapter.hpp"
+#include "adapter/camera_trigger_adapter.hpp"
+
+namespace robosense
+{
+namespace lidar
+{
+class AdapterManager
+{
+public:
+  AdapterManager() = default;
+  ~AdapterManager();
+  void init(const YAML::Node& config);
+  void start();
+  void stop();
+
+private:
+  std::shared_ptr<AdapterBase> createReceiver(const YAML::Node& config, const AdapterType& adapter_type);
+  std::shared_ptr<AdapterBase> createTransmitter(const YAML::Node& config, const AdapterType& adapter_type);
+
+private:
+  bool packet_thread_flag_;
+  bool point_cloud_thread_flag_;
+  std::vector<AdapterBase::Ptr> packet_receive_adapter_vec_;
+  std::vector<AdapterBase::Ptr> point_cloud_receive_adapter_vec_;
+  std::vector<AdapterBase::Ptr> packet_transmit_adapter_vec_;
+  std::vector<AdapterBase::Ptr> point_cloud_transmit_adapter_vec_;
+};
+}  // namespace lidar
+}  // namespace robosense

src/ros/open_source_code/rslidar_sdk/src/msg/proto_msg/packet.proto

@@ -0,0 +1,9 @@
+syntax="proto2";
+package proto_msg;
+
+message LidarPacket
+{ 
+ optional bytes data = 1;
+}
+
+

src/ros/open_source_code/rslidar_sdk/src/msg/proto_msg/point_cloud.proto

@@ -0,0 +1,16 @@
+syntax="proto2";
+package proto_msg;
+
+message LidarPointCloud
+{ 
+ optional double timestamp = 1; 
+ optional uint32 seq = 2;
+ optional string frame_id = 3;
+ optional uint32 height=4;
+ optional uint32 width=5;
+ optional bool is_dense=6;
+ repeated float data = 7;
+
+}
+
+

src/ros/open_source_code/rslidar_sdk/src/msg/proto_msg/scan.proto

@@ -0,0 +1,11 @@
+syntax="proto2";
+package proto_msg;
+
+message LidarScan
+{ 
+ optional double timestamp = 1; 
+ optional uint32 seq=2;
+ repeated bytes data = 3;
+}
+
+

src/ros/open_source_code/rslidar_sdk/src/msg/proto_msg_translator.h

@@ -0,0 +1,142 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#ifdef PROTO_FOUND
+#include "msg/rs_msg/lidar_point_cloud_msg.h"
+#include "rs_driver/msg/packet_msg.h"
+#include "rs_driver/msg/scan_msg.h"
+#include "msg/proto_msg/point_cloud.pb.h"
+#include "msg/proto_msg/packet.pb.h"
+#include "msg/proto_msg/scan.pb.h"
+namespace robosense
+{
+namespace lidar
+{
+/************************************************************************/
+/**Translation functions between RoboSense message and protobuf message**/
+/************************************************************************/
+inline proto_msg::LidarPointCloud toProtoMsg(const LidarPointCloudMsg& rs_msg)
+{
+  proto_msg::LidarPointCloud proto_msg;
+  proto_msg.set_timestamp(rs_msg.timestamp);
+  proto_msg.set_seq(rs_msg.seq);
+  proto_msg.set_frame_id(rs_msg.frame_id);
+  proto_msg.set_height(rs_msg.point_cloud_ptr->height);
+  proto_msg.set_width(rs_msg.point_cloud_ptr->width);
+  proto_msg.set_is_dense(rs_msg.point_cloud_ptr->is_dense);
+
+  for (size_t i = 0; i < rs_msg.point_cloud_ptr->size(); i++)
+  {
+    proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].x);
+    proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].y);
+    proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].z);
+    proto_msg.add_data(rs_msg.point_cloud_ptr->points[i].intensity);
+  }
+
+  return std::move(proto_msg);
+}
+
+inline LidarPointCloudMsg toRsMsg(const proto_msg::LidarPointCloud& proto_msg)
+{
+  LidarPointCloudMsg rs_msg;
+  rs_msg.timestamp = proto_msg.timestamp();
+  rs_msg.seq = proto_msg.seq();
+  rs_msg.frame_id = proto_msg.frame_id();
+  LidarPointCloudMsg::PointCloud* ptr_tmp = new LidarPointCloudMsg::PointCloud();
+  for (int i = 0; i < proto_msg.data_size(); i += 4)
+  {
+    PointT point;
+    point.x = proto_msg.data(i);
+    point.y = proto_msg.data(i + 1);
+    point.z = proto_msg.data(i + 2);
+    point.intensity = proto_msg.data(i + 3);
+    ptr_tmp->push_back(point);
+  }
+  ptr_tmp->height = proto_msg.height();
+  ptr_tmp->width = proto_msg.width();
+  ptr_tmp->is_dense = proto_msg.is_dense();
+  rs_msg.point_cloud_ptr.reset(ptr_tmp);
+  return rs_msg;
+}
+
+inline proto_msg::LidarScan toProtoMsg(const ScanMsg& rs_msg)
+{
+  proto_msg::LidarScan proto_msg;
+  proto_msg.set_timestamp(rs_msg.timestamp);
+  proto_msg.set_seq(rs_msg.seq);
+  for (auto iter : rs_msg.packets)
+  {
+    void* data_ptr = malloc(iter.packet.size());
+    memcpy(data_ptr, iter.packet.data(), iter.packet.size());
+    proto_msg.add_data(data_ptr, iter.packet.size());
+    free(data_ptr);
+  }
+  return proto_msg;
+}
+
+inline ScanMsg toRsMsg(const proto_msg::LidarScan& proto_msg)
+{
+  ScanMsg rs_msg;
+  rs_msg.timestamp = proto_msg.timestamp();
+  rs_msg.seq = proto_msg.seq();
+  for (int i = 0; i < proto_msg.data_size(); i++)
+  {
+    std::string data_str = proto_msg.data(i);
+    PacketMsg tmp_pkt(data_str.size());
+    memcpy(tmp_pkt.packet.data(), data_str.data(), data_str.size());
+    rs_msg.packets.emplace_back(std::move(tmp_pkt));
+  }
+  return rs_msg;
+}
+
+inline proto_msg::LidarPacket toProtoMsg(const PacketMsg& rs_msg)
+{
+  proto_msg::LidarPacket proto_msg;
+  void* data_ptr = malloc(rs_msg.packet.size());
+  memcpy(data_ptr, rs_msg.packet.data(), rs_msg.packet.size());
+  proto_msg.set_data(data_ptr, rs_msg.packet.size());
+  free(data_ptr);
+  return proto_msg;
+}
+
+inline PacketMsg toRsMsg(const proto_msg::LidarPacket& proto_msg)
+{
+  std::string data_str = proto_msg.data();
+  PacketMsg rs_msg(data_str.size());
+  memcpy(rs_msg.packet.data(), data_str.data(), data_str.size());
+  return rs_msg;
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // PROTO_FOUND

src/ros/open_source_code/rslidar_sdk/src/msg/ros_msg/lidar_packet_ros.h

@@ -0,0 +1,196 @@
+// Generated by gencpp from file rslidar_msgs/rslidarPacket.msg
+// DO NOT EDIT!
+
+#pragma once
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+namespace rslidar_msgs
+{
+template <class ContainerAllocator>
+struct rslidarPacket_
+{
+  typedef rslidarPacket_<ContainerAllocator> Type;
+
+  rslidarPacket_() : stamp(), data()
+  {
+    data.assign(0);
+  }
+  rslidarPacket_(const ContainerAllocator& _alloc) : stamp(), data()
+  {
+    (void)_alloc;
+    data.assign(0);
+  }
+
+  typedef ros::Time _stamp_type;
+  _stamp_type stamp;
+
+  typedef boost::array<uint8_t, 1248> _data_type;
+  _data_type data;
+
+  typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> > Ptr;
+  typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> const> ConstPtr;
+
+};  // struct rslidarPacket_
+
+typedef ::rslidar_msgs::rslidarPacket_<std::allocator<void> > rslidarPacket;
+
+typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket> rslidarPacketPtr;
+typedef boost::shared_ptr< ::rslidar_msgs::rslidarPacket const> rslidarPacketConstPtr;
+
+// constants requiring out of line definition
+
+template <typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::rslidar_msgs::rslidarPacket_<ContainerAllocator>& v)
+{
+  ros::message_operations::Printer< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> >::stream(s, "", v);
+  return s;
+}
+
+}  // namespace rslidar_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+// BOOLTRAITS {'IsFixedSize': true, 'IsMessage': true, 'HasHeader': false}
+// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'rslidar_msgs':
+// ['/home/xzd/catkin_ws/src/rs_driver/rslidar_msgs/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__',
+// '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__',
+// '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name',
+// 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> > : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct IsFixedSize< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> const> : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct IsMessage< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> > : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct IsMessage< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> const> : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct HasHeader< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> > : FalseType
+{
+};
+
+template <class ContainerAllocator>
+struct HasHeader< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> const> : FalseType
+{
+};
+
+template <class ContainerAllocator>
+struct MD5Sum< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "1e4288e00b9222ea477b73350bf24f51";
+  }
+
+  static const char* value(const ::rslidar_msgs::rslidarPacket_<ContainerAllocator>&)
+  {
+    return value();
+  }
+  static const uint64_t static_value1 = 0x1e4288e00b9222eaULL;
+  static const uint64_t static_value2 = 0x477b73350bf24f51ULL;
+};
+
+template <class ContainerAllocator>
+struct DataType< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "rslidar_msgs/rslidarPacket";
+  }
+
+  static const char* value(const ::rslidar_msgs::rslidarPacket_<ContainerAllocator>&)
+  {
+    return value();
+  }
+};
+
+template <class ContainerAllocator>
+struct Definition< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> >
+{
+  static const char* value()
+  {
+    return "# Raw LIDAR packet.\n\
+\n\
+timestamp              # packet timestamp\n\
+uint8[1248] data        # packet contents\n\
+\n\
+";
+  }
+
+  static const char* value(const ::rslidar_msgs::rslidarPacket_<ContainerAllocator>&)
+  {
+    return value();
+  }
+};
+
+}  // namespace message_traits
+}  // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+template <class ContainerAllocator>
+struct Serializer< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> >
+{
+  template <typename Stream, typename T>
+  inline static void allInOne(Stream& stream, T m)
+  {
+    stream.next(m.stamp);
+    stream.next(m.data);
+  }
+
+  ROS_DECLARE_ALLINONE_SERIALIZER
+};  // struct rslidarPacket_
+
+}  // namespace serialization
+}  // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+template <class ContainerAllocator>
+struct Printer< ::rslidar_msgs::rslidarPacket_<ContainerAllocator> >
+{
+  template <typename Stream>
+  static void stream(Stream& s, const std::string& indent, const ::rslidar_msgs::rslidarPacket_<ContainerAllocator>& v)
+  {
+    s << indent << "stamp: ";
+    Printer<ros::Time>::stream(s, indent + "  ", v.stamp);
+    s << indent << "data[]" << std::endl;
+    for (size_t i = 0; i < v.data.size(); ++i)
+    {
+      s << indent << "  data[" << i << "]: ";
+      Printer<uint8_t>::stream(s, indent + "  ", v.data[i]);
+    }
+  }
+};
+
+}  // namespace message_operations
+}  // namespace ros

src/ros/open_source_code/rslidar_sdk/src/msg/ros_msg/lidar_scan_ros.h

@@ -0,0 +1,228 @@
+// Generated by gencpp from file rslidar_msgs/rslidarScan.msg
+// DO NOT EDIT!
+
+#pragma once
+
+#include <string>
+#include <vector>
+#include <map>
+
+#include <ros/types.h>
+#include <ros/serialization.h>
+#include <ros/builtin_message_traits.h>
+#include <ros/message_operations.h>
+
+#include <std_msgs/Header.h>
+#include "msg/ros_msg/lidar_packet_ros.h"
+
+namespace rslidar_msgs
+{
+template <class ContainerAllocator>
+struct rslidarScan_
+{
+  typedef rslidarScan_<ContainerAllocator> Type;
+
+  rslidarScan_() : header(), packets()
+  {
+  }
+  rslidarScan_(const ContainerAllocator& _alloc) : header(_alloc), packets(_alloc)
+  {
+    (void)_alloc;
+  }
+
+  typedef ::std_msgs::Header_<ContainerAllocator> _header_type;
+  _header_type header;
+
+  typedef std::vector<
+      ::rslidar_msgs::rslidarPacket_<ContainerAllocator>,
+      typename ContainerAllocator::template rebind<::rslidar_msgs::rslidarPacket_<ContainerAllocator>>::other>
+      _packets_type;
+  _packets_type packets;
+
+  typedef boost::shared_ptr<::rslidar_msgs::rslidarScan_<ContainerAllocator>> Ptr;
+  typedef boost::shared_ptr<::rslidar_msgs::rslidarScan_<ContainerAllocator> const> ConstPtr;
+
+};  // struct rslidarScan_
+
+typedef ::rslidar_msgs::rslidarScan_<std::allocator<void>> rslidarScan;
+
+typedef boost::shared_ptr<::rslidar_msgs::rslidarScan> rslidarScanPtr;
+typedef boost::shared_ptr<::rslidar_msgs::rslidarScan const> rslidarScanConstPtr;
+
+// constants requiring out of line definition
+
+template <typename ContainerAllocator>
+std::ostream& operator<<(std::ostream& s, const ::rslidar_msgs::rslidarScan_<ContainerAllocator>& v)
+{
+  ros::message_operations::Printer<::rslidar_msgs::rslidarScan_<ContainerAllocator>>::stream(s, "", v);
+  return s;
+}
+
+}  // namespace rslidar_msgs
+
+namespace ros
+{
+namespace message_traits
+{
+// BOOLTRAITS {'IsFixedSize': false, 'IsMessage': true, 'HasHeader': true}
+// {'std_msgs': ['/opt/ros/kinetic/share/std_msgs/cmake/../msg'], 'rslidar_msgs':
+// ['/home/xzd/catkin_ws/src/rs_driver/rslidar_msgs/msg']}
+
+// !!!!!!!!!!! ['__class__', '__delattr__', '__dict__', '__doc__', '__eq__', '__format__', '__getattribute__',
+// '__hash__', '__init__', '__module__', '__ne__', '__new__', '__reduce__', '__reduce_ex__', '__repr__', '__setattr__',
+// '__sizeof__', '__str__', '__subclasshook__', '__weakref__', '_parsed_fields', 'constants', 'fields', 'full_name',
+// 'has_header', 'header_present', 'names', 'package', 'parsed_fields', 'short_name', 'text', 'types']
+
+template <class ContainerAllocator>
+struct IsFixedSize<::rslidar_msgs::rslidarScan_<ContainerAllocator>> : FalseType
+{
+};
+
+template <class ContainerAllocator>
+struct IsFixedSize<::rslidar_msgs::rslidarScan_<ContainerAllocator> const> : FalseType
+{
+};
+
+template <class ContainerAllocator>
+struct IsMessage<::rslidar_msgs::rslidarScan_<ContainerAllocator>> : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct IsMessage<::rslidar_msgs::rslidarScan_<ContainerAllocator> const> : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct HasHeader<::rslidar_msgs::rslidarScan_<ContainerAllocator>> : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct HasHeader<::rslidar_msgs::rslidarScan_<ContainerAllocator> const> : TrueType
+{
+};
+
+template <class ContainerAllocator>
+struct MD5Sum<::rslidar_msgs::rslidarScan_<ContainerAllocator>>
+{
+  static const char* value()
+  {
+    return "ff6baa58985b528481871cbaf1bb342f";
+  }
+
+  static const char* value(const ::rslidar_msgs::rslidarScan_<ContainerAllocator>&)
+  {
+    return value();
+  }
+  static const uint64_t static_value1 = 0xff6baa58985b5284ULL;
+  static const uint64_t static_value2 = 0x81871cbaf1bb342fULL;
+};
+
+template <class ContainerAllocator>
+struct DataType<::rslidar_msgs::rslidarScan_<ContainerAllocator>>
+{
+  static const char* value()
+  {
+    return "rslidar_msgs/rslidarScan";
+  }
+
+  static const char* value(const ::rslidar_msgs::rslidarScan_<ContainerAllocator>&)
+  {
+    return value();
+  }
+};
+
+template <class ContainerAllocator>
+struct Definition<::rslidar_msgs::rslidarScan_<ContainerAllocator>>
+{
+  static const char* value()
+  {
+    return "# LIDAR scan packets.\n\
+\n\
+Header           header         # standard ROS message header\n\
+rslidarPacket[] packets        # vector of raw packets\n\
+\n\
+================================================================================\n\
+MSG: std_msgs/Header\n\
+# Standard metadata for higher-level stamped data types.\n\
+# This is generally used to communicate timestamped data \n\
+# in a particular coordinate frame.\n\
+# \n\
+# sequence ID: consecutively increasing ID \n\
+uint32 seq\n\
+#Two-integer timestamp that is expressed as:\n\
+# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n\
+# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n\
+# time-handling sugar is provided by the client library\n\
+timestamp\n\
+#Frame this data is associated with\n\
+# 0: no frame\n\
+# 1: global frame\n\
+string frame_id\n\
+\n\
+================================================================================\n\
+MSG: rslidar_msgs/rslidarPacket\n\
+# Raw LIDAR packet.\n\
+\n\
+timestamp              # packet timestamp\n\
+uint8[1248] data        # packet contents\n\
+\n\
+";
+  }
+
+  static const char* value(const ::rslidar_msgs::rslidarScan_<ContainerAllocator>&)
+  {
+    return value();
+  }
+};
+
+}  // namespace message_traits
+}  // namespace ros
+
+namespace ros
+{
+namespace serialization
+{
+template <class ContainerAllocator>
+struct Serializer<::rslidar_msgs::rslidarScan_<ContainerAllocator>>
+{
+  template <typename Stream, typename T>
+  inline static void allInOne(Stream& stream, T m)
+  {
+    stream.next(m.header);
+    stream.next(m.packets);
+  }
+
+  ROS_DECLARE_ALLINONE_SERIALIZER
+};  // struct rslidarScan_
+
+}  // namespace serialization
+}  // namespace ros
+
+namespace ros
+{
+namespace message_operations
+{
+template <class ContainerAllocator>
+struct Printer<::rslidar_msgs::rslidarScan_<ContainerAllocator>>
+{
+  template <typename Stream>
+  static void stream(Stream& s, const std::string& indent, const ::rslidar_msgs::rslidarScan_<ContainerAllocator>& v)
+  {
+    s << indent << "header: ";
+    s << std::endl;
+    Printer<::std_msgs::Header_<ContainerAllocator>>::stream(s, indent + "  ", v.header);
+    s << indent << "packets[]" << std::endl;
+    for (size_t i = 0; i < v.packets.size(); ++i)
+    {
+      s << indent << "  packets[" << i << "]: ";
+      s << std::endl;
+      s << indent;
+      Printer<::rslidar_msgs::rslidarPacket_<ContainerAllocator>>::stream(s, indent + "    ", v.packets[i]);
+    }
+  }
+};
+
+}  // namespace message_operations
+}  // namespace ros

src/ros/open_source_code/rslidar_sdk/src/msg/ros_msg_translator.h

@@ -0,0 +1,224 @@
+/*********************************************************************************************************************
+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.
+*********************************************************************************************************************/
+
+#pragma once
+#ifdef ROS_FOUND
+#include <std_msgs/Time.h>
+#include <ros/duration.h>
+#include <ros/rate.h>
+#include <ros/ros.h>
+#include <sensor_msgs/PointCloud2.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
+#include "msg/rs_msg/lidar_point_cloud_msg.h"
+#include "msg/ros_msg/lidar_packet_ros.h"
+#include "msg/ros_msg/lidar_scan_ros.h"
+#include "rs_driver/msg/packet_msg.h"
+#include "rs_driver/msg/scan_msg.h"
+#include "rs_driver/driver/driver_param.h"
+
+namespace robosense
+{
+namespace lidar
+{
+/************************************************************************/
+/**Translation functions between RoboSense message and ROS message**/
+/************************************************************************/
+inline sensor_msgs::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg)
+{
+  sensor_msgs::PointCloud2 ros_msg;
+  pcl::toROSMsg(*rs_msg.point_cloud_ptr, ros_msg);
+  ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp);
+  ros_msg.header.frame_id = rs_msg.frame_id;
+  ros_msg.header.seq = rs_msg.seq;
+  return std::move(ros_msg);
+}
+inline PacketMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type,
+                         const rslidar_msgs::rslidarPacket& ros_msg)
+{
+  size_t pkt_length;
+  switch (lidar_type)
+  {
+    case LidarType::RSM1:
+      if (pkt_type == PktType::MSOP)
+      {
+        pkt_length = MEMS_MSOP_LEN;
+      }
+      else
+      {
+        pkt_length = MEMS_DIFOP_LEN;
+      }
+      break;
+    default:
+      pkt_length = MECH_PKT_LEN;
+      break;
+  }
+  PacketMsg rs_msg(pkt_length);
+  for (size_t i = 0; i < pkt_length; i++)
+  {
+    rs_msg.packet[i] = ros_msg.data[i];
+  }
+  return std::move(rs_msg);
+}
+inline rslidar_msgs::rslidarPacket toRosMsg(const PacketMsg& rs_msg)
+{
+  rslidar_msgs::rslidarPacket ros_msg;
+  for (size_t i = 0; i < rs_msg.packet.size(); i++)
+  {
+    ros_msg.data[i] = rs_msg.packet[i];
+  }
+  return std::move(ros_msg);
+}
+inline ScanMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type, const rslidar_msgs::rslidarScan& ros_msg)
+{
+  ScanMsg rs_msg;
+  rs_msg.seq = ros_msg.header.seq;
+  rs_msg.timestamp = ros_msg.header.stamp.toSec();
+  rs_msg.frame_id = ros_msg.header.frame_id;
+
+  for (uint32_t i = 0; i < ros_msg.packets.size(); i++)
+  {
+    PacketMsg tmp = toRsMsg(lidar_type, pkt_type, ros_msg.packets[i]);
+    rs_msg.packets.emplace_back(std::move(tmp));
+  }
+  return std::move(rs_msg);
+}
+inline rslidar_msgs::rslidarScan toRosMsg(const ScanMsg& rs_msg)
+{
+  rslidar_msgs::rslidarScan ros_msg;
+  ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp);
+  ros_msg.header.frame_id = rs_msg.frame_id;
+  ros_msg.header.seq = rs_msg.seq;
+  for (uint32_t i = 0; i < rs_msg.packets.size(); i++)
+  {
+    rslidar_msgs::rslidarPacket tmp = toRosMsg(rs_msg.packets[i]);
+    ros_msg.packets.emplace_back(std::move(tmp));
+  }
+  return std::move(ros_msg);
+}
+inline std_msgs::Time toRosMsg(const CameraTrigger& rs_msg)
+{
+  std_msgs::Time ros_msg;
+  ros_msg.data = ros_msg.data.fromSec(rs_msg.second);
+  return ros_msg;
+}
+
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS_FOUND
+
+#ifdef ROS2_FOUND
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/msg/point_cloud2.hpp>
+#include "rslidar_msg/msg/rslidar_packet.hpp"
+#include "rslidar_msg/msg/rslidar_scan.hpp"
+namespace robosense
+{
+namespace lidar
+{
+/************************************************************************/
+/**Translation functions between RoboSense message and ROS2 message**/
+/************************************************************************/
+inline sensor_msgs::msg::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg)
+{
+  sensor_msgs::msg::PointCloud2 ros_msg;
+  pcl::toROSMsg(*rs_msg.point_cloud_ptr, ros_msg);
+  ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp);
+  ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9);
+  ros_msg.header.frame_id = rs_msg.frame_id;
+  return std::move(ros_msg);
+}
+inline PacketMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type,
+                         const rslidar_msg::msg::RslidarPacket& ros_msg)
+{
+  size_t pkt_length;
+  switch (lidar_type)
+  {
+    case LidarType::RSM1:
+      if (pkt_type == PktType::MSOP)
+      {
+        pkt_length = MEMS_MSOP_LEN;
+      }
+      else
+      {
+        pkt_length = MEMS_DIFOP_LEN;
+      }
+      break;
+    default:
+      pkt_length = MECH_PKT_LEN;
+      break;
+  }
+  PacketMsg rs_msg(pkt_length);
+  for (size_t i = 0; i < pkt_length; i++)
+  {
+    rs_msg.packet[i] = ros_msg.data[i];
+  }
+  return std::move(rs_msg);
+}
+inline rslidar_msg::msg::RslidarPacket toRosMsg(const PacketMsg& rs_msg)
+{
+  rslidar_msg::msg::RslidarPacket ros_msg;
+  for (size_t i = 0; i < rs_msg.packet.size(); i++)
+  {
+    ros_msg.data[i] = rs_msg.packet[i];
+  }
+  return std::move(ros_msg);
+}
+inline ScanMsg toRsMsg(const LidarType& lidar_type, const PktType& pkt_type,
+                       const rslidar_msg::msg::RslidarScan& ros_msg)
+{
+  ScanMsg rs_msg;
+  rs_msg.timestamp = ros_msg.header.stamp.sec + double(ros_msg.header.stamp.nanosec) / 1e9;
+  rs_msg.frame_id = ros_msg.header.frame_id;
+  for (uint32_t i = 0; i < ros_msg.packets.size(); i++)
+  {
+    PacketMsg tmp = toRsMsg(lidar_type, pkt_type, ros_msg.packets[i]);
+    rs_msg.packets.emplace_back(std::move(tmp));
+  }
+  return rs_msg;
+}
+inline rslidar_msg::msg::RslidarScan toRosMsg(const ScanMsg& rs_msg)
+{
+  rslidar_msg::msg::RslidarScan ros_msg;
+  ros_msg.header.stamp.sec = (uint32_t)floor(rs_msg.timestamp);
+  ros_msg.header.stamp.nanosec = (uint32_t)round((rs_msg.timestamp - ros_msg.header.stamp.sec) * 1e9);
+  ros_msg.header.frame_id = rs_msg.frame_id;
+  for (uint32_t i = 0; i < rs_msg.packets.size(); i++)
+  {
+    rslidar_msg::msg::RslidarPacket tmp = toRosMsg(rs_msg.packets[i]);
+    ros_msg.packets.emplace_back(std::move(tmp));
+  }
+  return ros_msg;
+}
+}  // namespace lidar
+}  // namespace robosense
+#endif  // ROS2_FOUND