houyongdi/dreamDeckAutopilot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Initial commit

Browse Source
This commit is contained in:
houyongdi
2026-07-27 13:51:19 +08:00
commit 7bec56ca51
5408 changed files with 1126933 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sensors/lslidar_C16/README.md
+288
View File
@@ -0,0 +1,288 @@
# lslidar_c16
#version v3.1.1_210922
## version track
Author: zx
### ver3.0 zx
## Description
The `lslidar_c16` package is a linux ROS driver for lslidar c16.
The package is tested on Ubuntu 16.04/18.04 with ROS kinetic/melodic.
## Compling
This is a Catkin package. Make sure the package is on `ROS_PACKAGE_PATH` after cloning the package to your workspace. And the normal procedure for compling a catkin package will work.
```
cd your_work_space
catkin_make
```
## Example Usage
### lslidar_c16_decoder
**Parameters**
`lidar_ip` (`string`, `default: 192.168.1.200`)
By default, the IP address of the device is 192.168.1.200.
`frame_id` (`string`, `default: laser_link`)
The frame ID entry for the sent messages.
**Published Topics**
`lslidar_point_cloud`
Each message corresponds to a lslidar packet sent by the device through the Ethernet.
### lslidar_c16_decoder
**Parameters**
`min_range` (`double`, `0.3`)
`max_range` (`double`, `200.0`)
Points outside this range will be removed.
`frequency` (`frequency`, `10.0`)
Note that the driver does not change the frequency of the sensor.
`publish_point_cloud` (`bool`, `true`)
If set to true, the decoder will additionally send out a local point cloud consisting of the points in each revolution.
**Published Topics**
`lslidar_sweep` (`lslidar_c16_msgs/LslidarChSweep`)
The message arranges the points within each sweep based on its scan index and azimuth.
`lslidar_pointclou_c16` (`sensor_msgs/PointCloud2`)
This is only published when the `publish_point_cloud` is set to `true` in the launch file.
**Node**
```
roslaunch lslidar_c16_decoder lslidar_c16.launch --screen
```
Note that this launch file launches both the driver and the decoder, which is the only launch file needed to be used.
## FAQ
## Bug Report
##Version changes
/***********2020-01-03****************/
Original version : lslidar_c16_v2.02_190919
Revised version : lslidar_c16_v2.03_200103
Modify : Add a new calibration decode for the new lslidar c16
Author : zx
Date : 2020-01-03
/***********2020-01-16****************/
Original version : lslidar_c16_v2.03_200103
Revised version : lslidar_c16_v2.6.0_200116
Modify : Adds the vertical Angle correction file lslidar_c16_db.yaml for the RoS program code
Change the range resolution to 0.25cm according to the v2.6 protocol
Author : zx
Date : 2020-01-16
/***********2020-04-02****************/
Original version : lslidar_c16_v2.6.0_200116
Revised version : lslidar_c16_v2.6.1_200402
Modify : 1. 增加了读取设备包并解析垂直角度值的功能，用于替换原有固定的垂直角度值。
2. 修改了lslidar_c16.launch文件，用于兼容选择参数和功能
Author : zx
Date : 2020-04-02
luanch文件说明:
<node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen">
<param name="calibration_file" value="$(find lslidar_c16_decoder)/params/lslidar_c16_db.yaml" />
<param name="min_range" value="0.15"/>
<param name="max_range" value="150.0"/>
<param name="cbMethod" value="true"/> //cbMethod = true表示增加x,y坐标的偏移计算补偿, false则不加
<param name="print_vert" value="true"/> //print_vert = true 表示打印设备包角度信息， false表示关闭打印信息
<param name="config_vert_file" value="false"/> //config_vert_file = true 表示读取yaml文件中的垂直角度， false则关闭
<param name="distance_unit" value="0.25"/> //distance_unit = 0.25表示距离单位为0.25cm, = 1表示距离单位为1cm
<param name="time_synchronization" value="$(arg time_synchronization)"/>
</node>
/***********2020-06-06************/
Original version : lslidar_c16_v2.6.1_200402
Revised version : lslidar_c16_v3.0_200606
Modify : 1. 修改了读取设备包并解析垂直角度值和GPS时间的功能。
​ 2. 增加了读取设备包并解析3号版版本号，用于兼容V2.0版本雷达。
Author : lqm
Date : 2020-06-06
/***********2020-06-19************/
Original version : lslidar_c16_v3.0_200606
Revised version : lslidar_c16_v3.0.1_200619
Modify : 1. 点云校准距离计算方式更新。
Author : lqm
Date : 2020-06-19
/***********2020-08-18************/
Original version : lslidar_c16_v3.0.1_200619
Revised version : LSLIDAR_C16_V3.0.2_200818_ROSK
Modify : 1. 增加点云中每个点的线号信息。
Author : lqm
Date : 2020-08-18
/***********2020-08-26************/
Original version : LSLIDAR_C16_V3.0.2_200818_ROSK
Revised version : LSLIDAR_C16_V3.0.3_200826_ROSK
Modify : 1. 更新每一帧点云的起始角度固定在0°附近，结束角度在360°。
Author : lqm
Date : 2020-08-26
/***********2020-09-10************/
Original version : LSLIDAR_C16_V3.0.3_200826_ROSK
Revised version : LSLIDAR_C16_V3.0.4_200910_ROSK
Modify :
1. 新增兼容垂直角度分辨率为1.33°雷达。
2. 增加LaserScan 消息类型发布。
3. 新增扫描角度裁剪。
4. 更新gps时间解析，秒+1。
launch文件说明：
​ <param name="degree_mode" value="1"/> <!--1表示垂直角度分辨率为1.33°，2表示垂直角度分辨率为2° -->
<param name="scan_start_angle" value="0.0"/> <!-- 扫描裁剪起始角-->
​ <param name="scan_end_angle" value="36000.0"/> <!-- 扫描裁剪结束角，单位0.01°-->
​ <param name="scan_num" value="8"/> <!--LaserScan选择的通道-->
​ <param name="publish_scan" value="false"/> <!--是否发布LaserScan消息类型-->
Author : lqm
Date : 2020-09-10
/***********2020-12-02************/
Original version : LSLIDAR_C16_V3.0.4_200910_ROSK
Revised version : LSLIDAR_C16_V3.0.6_201202_ROSK
Modify :
1. 兼容 Ubuntu18.04的ROS melodic。
2. 新增nodelet.launch文件。
3. 去除线号（lines/ring）信息，使用标准点云数据类型。
launch文件说明：
​ <arg name="time_synchronization" default="false" />
<！--默认 ，时间同步为false。如果接GPS模块授时，则需改为 true。-->
Author : lqm
Date : 2020-12-02
/***********2021-04-08************/
Original version : LSLIDAR_C16_V3.0.6_201202_ROSK
Revised version : LSLIDAR_C16_V3.0.8_210408_ROS
Modify :
1. 新增可以单独显示第二次回波点云模式，只针对双回波雷达有效。
2. 增加静态坐标转换示例。
```xml
lslidar_c16.launch文件说明：
<arg name="return_mode" default="1" />
<param name="echo_second" value="false"/>
<!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" /-->
```
若需要只显示第二次回波点云的模式，参数改为如下：
<arg name="return_mode" default="2" />
<param name="echo_second" value="true"/>
静态坐标转换示例，取消注释。
<node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" >
并根据实际需求 ，调整 0 0 0 0 0 0 这6个参数值（XYZ和xyz轴旋转角度）。
Author : lqm
Date : 2021-04-08
/***********2021-07-01************/
Original version : LSLIDAR_C16_V3.0.8_210408_ROS
Revised version : LSLIDAR_C16_V3.0.9_210701_ROS
Modify :
1. 新增启动launch文件时，打开rviz。
2. 添加了service通信控制上下电的功能。
另开终端，输入以下命令:
```shell
source devel/setup.bash
rosservice call /lslidarcontrol 1 //上电
rosservice call /lslidarcontrol 0 //下电
```
Author : lqm
Date : 2021-07-01
/***********2021-07-26************/
Original version : LSLIDAR_C16_V3.0.9_210701_ROS
Revised version : LSLIDAR_C16_V3.1.0_210726_ROS
Modify :
1. 新增点的线号和时间戳。
2. 增加install编译。
3. 修改同一工作空间下编译不同雷达驱动报错问题。
Author : lqm
Date : 2021-07-26
2021-09-22
原始版本：LSLIDAR_C16_V3.1.0_210726_ROS
修订版本：LSLIDAR_C16_V3.1.1_210922_ROS
更新内容：
1、增加坐标系选择，默认雷达0度角对应x轴
launch 文件说明：
~~~xml
<arg name="time_synchronization" default="false" /> //默认未开启GPS授时
<arg name="return_mode" default="1" /> //默认单回波，双回波雷达改为2
<param name="degree_mode" value="2"/> //雷达垂直角度分辨率，默认2代表2度分辨率，1代表1.33度分辨率
<param name="distance_unit" value="0.25"/> //距离分辨率0.25
<param name="pointcloud_topic" value="lslidar_point_cloud"/> //lslidar_point_cloud为点云话题名，可改
<param name="coordinate_opt" value="true"/> // 坐标系，默认雷达0度角对应x轴，false为雷达0度角对应y轴
~~~
sensors/lslidar_C16/lslidar_c16/CMakeLists.txt
+4
View File
@@ -0,0 +1,4 @@
cmake_minimum_required(VERSION 2.8.3)
project(lslidar_c16)
find_package(catkin REQUIRED)
catkin_metapackage()
sensors/lslidar_C16/lslidar_c16/package.xml
+25
View File
@@ -0,0 +1,25 @@
<package>
<name>lslidar_c16</name>
<version>1.2.0</version>
<description>
Basic ROS support for the Leishen C16 LIDARs.
</description>
<url>http://www.ros.org/wiki/lslidar_c16</url>
<maintainer email="yutongyan@lslidar.com">Yutong</maintainer>
<author>Yutong</author>
<license>GNU General Public License V3.0</license>
<buildtool_depend>catkin</buildtool_depend>
<run_depend>lslidar_c16_driver</run_depend>
<run_depend>lslidar_c16_msgs</run_depend>
<run_depend>lslidar_c16_decoder</run_depend>
<run_depend>lslidar_c16_sys</run_depend>
<export>
<metapackage/>
</export>
</package>
sensors/lslidar_C16/lslidar_c16_decoder/CMakeLists.txt
+99
View File
@@ -0,0 +1,99 @@
cmake_minimum_required(VERSION 2.8.3)
project(lslidar_c16_decoder)
add_compile_options(-std=c++14)
#set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
#message(STATUS "CMAKE: ${CMAKE_MODULE_PATH}")
find_package(catkin REQUIRED COMPONENTS
angles
nodelet
pcl_ros
roscpp
sensor_msgs
tf
lslidar_c16_driver
lslidar_c16_msgs
dynamic_reconfigure
pcl_conversions)
find_package(Boost COMPONENTS signals)
find_package(PkgConfig REQUIRED)
find_package(yaml-cpp REQUIRED)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES lslidar_c16_decoder
CATKIN_DEPENDS
angles
nodelet
pcl_ros
roscpp
sensor_msgs
tf
lslidar_c16_driver
lslidar_c16_msgs
dynamic_reconfigure
pcl_conversions
DEPENDS
Boost
)
include_directories(
include
${catkin_INCLUDE_DIRS}
${Boost_INCLUDE_DIR}
${YAML_CPP_INCLUDE_DIRS}
)
add_library(lslidar_data src/rawdata.cc)
target_link_libraries(lslidar_data
${catkin_LIBRARIES}
${libpcap_LIBRARIES}
yaml-cpp)
add_library(lslidar_point src/convert.cc)
target_link_libraries(lslidar_point
lslidar_data
${catkin_LIBRARIES})
add_library(lslidar_c16_decoder_nodelet src/cloud_nodelet.cc)
target_link_libraries(lslidar_c16_decoder_nodelet lslidar_point
${catkin_LIBRARIES})
add_executable(lslidar_c16_decoder_node src/cloud_node.cc)
target_link_libraries(lslidar_c16_decoder_node
lslidar_point
${catkin_LIBRARIES}
${libpcap_LIBRARIES})
add_dependencies(lslidar_c16_decoder_node
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
if(catkin_EXPORTED_TARGETS)
add_dependencies(lslidar_data ${catkin_EXPORTED_TARGETS})
endif()
install(TARGETS lslidar_data lslidar_point lslidar_c16_decoder_nodelet lslidar_c16_decoder_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES
nodelets.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
sensors/lslidar_C16/lslidar_c16_decoder/include/lslidar_c16_decoder/convert.h
+67
View File
@@ -0,0 +1,67 @@
/*
* This file is part of lslidar_n301 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _CONVERT_H_
#define _CONVERT_H_
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <dynamic_reconfigure/server.h>
#include "rawdata.h"
#include <sensor_msgs/TimeReference.h>
namespace lslidar_c16_decoder {
class Convert {
public:
Convert(ros::NodeHandle node, ros::NodeHandle private_nh);
~Convert() {
}
private:
void processScan(const lslidar_c16_msgs::LslidarC16ScanUnified::ConstPtr &scanMsg);
void timeSync(const sensor_msgs::TimeReferenceConstPtr &time_msg);
/// Pointer to dynamic reconfigure service srv_
void removeNan(lslidar_rawdata::VPointCloud &cloud_in,lslidar_rawdata::VPointCloud &cloud_out,std::vector<int> &index);
void publishScan(lslidar_c16_msgs::LslidarC16SweepPtr& sweepData, int scanNum);
boost::shared_ptr<lslidar_rawdata::RawData> data_;
ros::Subscriber packet_sub_;
ros::Subscriber sync_sub_;
ros::Time global_time;
double last_time;
lslidar_c16_msgs::LslidarC16ScanUnifiedPtr scan_recv;
lslidar_c16_msgs::LslidarC16SweepPtr sweep_data;
bool scan_start;
bool publish_scan;
int scan_num;
std::string scan_frame_id;
std::string pointcloud_topic;
size_t scan_nums;
bool time_synchronization_;
ros::Publisher output_;
ros::Publisher scan_pub;
std::vector<int> indices;
ros::Time scan_timestamp;
int frequency;
double horizontal_angle_resolution;
};
} // namespace lslidar_c16_decoder
#endif
sensors/lslidar_C16/lslidar_c16_decoder/include/lslidar_c16_decoder/rawdata.h
+262
View File
@@ -0,0 +1,262 @@
/*
* This file is part of lslidar_n301 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _RAWDATA_H
#define _RAWDATA_H
#include <ros/ros.h>
#include <ros/package.h>
#include <lslidar_c16_msgs/LslidarC16Packet.h>
#include <lslidar_c16_msgs/LslidarC16ScanUnified.h>
#include <lslidar_c16_msgs/LslidarC16Sweep.h>
#include "std_msgs/String.h"
#include <pcl/point_types.h>
#include <pcl_ros/point_cloud.h>
#include <pcl_ros/impl/transforms.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include <stdio.h>
#define DEG_TO_RAD 0.017453292
#define RAD_TO_DEG 57.29577951
namespace lslidar_rawdata {
// static const float ROTATION_SOLUTION_ = 0.18f; //水平角分辨率 10hz
static const int SIZE_BLOCK = 100;
static const int RAW_SCAN_SIZE = 3;
static const int SCANS_PER_BLOCK = 32;
static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE); // 96
static const float ROTATION_RESOLUTION = 0.01f; /**< degrees 旋转角分辨率*/
static const uint16_t ROTATION_MAX_UNITS = 36000; /**< hundredths of degrees */
static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000;
static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12;
static const float DISTANCE_MAX = 200.0f; /**< meters */
static const float DISTANCE_MIN = 0.2f; /**< meters */
static const float DISTANCE_RESOLUTION = 0.01f; /**< meters */
static const float DISTANCE_RESOLUTION_NEW = 0.005f; /**< meters */
static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX / DISTANCE_RESOLUTION + 1.0f);
/** @todo make this work for both big and little-endian machines */
static const uint16_t UPPER_BANK = 0xeeff; //
static const uint16_t LOWER_BANK = 0xddff;
/** Special Defines for LSC16 support **/
static const int LSC16_FIRINGS_PER_BLOCK = 2;
static const int LSC16_SCANS_PER_FIRING = 16;
static const float LSC16_BLOCK_TDURATION = 100.0f; // [µs]
static const float LSC16_DSR_TOFFSET = 3.125f; // [µs]
static const float LSC16_FIRING_TOFFSET = 50.0f; // [µs]
static const int TEMPERATURE_MIN = 31;
/** \brief Raw LSLIDAR C16 data block.
*
* Each block contains data from either the upper or lower laser
* bank. The device returns three times as many upper bank blocks.
*
* use stdint.h types, so things work with both 64 and 32-bit machines
*/
// block
typedef struct raw_block {
uint16_t header; ///< UPPER_BANK or LOWER_BANK
uint8_t rotation_1;
uint8_t rotation_2; /// combine rotation1 and rotation2 together to get 0-35999, divide by 100 to get degrees
uint8_t data[BLOCK_DATA_SIZE]; // 96
} raw_block_t;
struct PointXYZIRT {
PCL_ADD_POINT4D;
PCL_ADD_INTENSITY;
uint16_t ring;
double time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // make sure our new allocators are aligned
} EIGEN_ALIGN16;
// enforce SSE padding for correct memory alignment
/** used for unpacking the first two data bytes in a block
*
* They are packed into the actual data stream misaligned. I doubt
* this works on big endian machines.
*/
union two_bytes {
uint16_t uint;
uint8_t bytes[2];
};
// Pre-compute the sine and cosine for the altitude angles.
static const double scan_altitude_original_2[16] = {
-0.2617993877991494, 0.017453292519943295,
-0.22689280275926285, 0.05235987755982989,
-0.19198621771937624, 0.08726646259971647,
-0.15707963267948966, 0.12217304763960307,
-0.12217304763960307, 0.15707963267948966,
-0.08726646259971647, 0.19198621771937624,
-0.05235987755982989, 0.22689280275926285,
-0.017453292519943295, 0.2617993877991494
};
static const double scan_altitude_original_1[16] = {
-0.17453292519943295,0.01160643952576229,
-0.15123277968530863,0.03490658503988659,
-0.12793263417118436,0.05811946409141117,
-0.10471975511965978,0.08141960960553547,
-0.08141960960553547,0.10471975511965978,
-0.05811946409141117,0.12793263417118436,
-0.03490658503988659,0.15123277968530863,
-0.01160643952576229,0.17453292519943295
};
double scan_altitude[16];
static const double cos_scan_altitude[16] = {
std::cos(scan_altitude[0]), std::cos(scan_altitude[1]),
std::cos(scan_altitude[2]), std::cos(scan_altitude[3]),
std::cos(scan_altitude[4]), std::cos(scan_altitude[5]),
std::cos(scan_altitude[6]), std::cos(scan_altitude[7]),
std::cos(scan_altitude[8]), std::cos(scan_altitude[9]),
std::cos(scan_altitude[10]), std::cos(scan_altitude[11]),
std::cos(scan_altitude[12]), std::cos(scan_altitude[13]),
std::cos(scan_altitude[14]), std::cos(scan_altitude[15]),
};
static const double sin_scan_altitude[16] = {
std::sin(scan_altitude[0]), std::sin(scan_altitude[1]),
std::sin(scan_altitude[2]), std::sin(scan_altitude[3]),
std::sin(scan_altitude[4]), std::sin(scan_altitude[5]),
std::sin(scan_altitude[6]), std::sin(scan_altitude[7]),
std::sin(scan_altitude[8]), std::sin(scan_altitude[9]),
std::sin(scan_altitude[10]), std::sin(scan_altitude[11]),
std::sin(scan_altitude[12]), std::sin(scan_altitude[13]),
std::sin(scan_altitude[14]), std::sin(scan_altitude[15]),
};
static const int PACKET_SIZE = 1206;
static const int BLOCKS_PER_PACKET = 12;
static const int PACKET_STATUS_SIZE = 4;
static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
/** \brief Raw lslidar packet.
*
* revolution is described in the device manual as incrementing
* (mod 65536) for each physical turn of the device. Our device
* seems to alternate between two different values every third
* packet. One value increases, the other decreases.
*
* \todo figure out if revolution is only present for one of the
* two types of status fields
*
* status has either a temperature encoding or the microcode level
*/
typedef struct raw_packet {
raw_block_t blocks[BLOCKS_PER_PACKET];
uint16_t revolution;
uint8_t status[PACKET_STATUS_SIZE];
} raw_packet_t;
union vertical_point {
uint8_t uint[2];
uint16_t value;
};
typedef PointXYZIRT VPoint;
typedef pcl::PointCloud<VPoint> VPointCloud;
/** \brief lslidar data conversion class */
class RawData {
public:
RawData();
~RawData() {
}
/*load the cablibrated files: angle, distance, intensity*/
void loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh);
/*unpack the UDP packet and opuput PCL PointXYZI type*/
void unpack(const lslidar_c16_msgs::LslidarC16Packet &pkt, VPointCloud::Ptr pointcloud,int Packet_num,lslidar_c16_msgs::LslidarC16SweepPtr& sweep_data);
void processDifop(const lslidar_c16_msgs::LslidarC16Packet::ConstPtr &difop_msg);
ros::Subscriber difop_sub_;
bool is_init_curve_;
bool is_init_angle_;
bool is_init_top_fw_;
int block_num = 0;
int intensity_mode_;
int intensityFactor;
private:
float R1_;
float R2_;
bool angle_flag_;
float start_angle_;
float end_angle_;
float max_distance_;
float min_distance_;
float distance_unit_;
int dis_resolution_mode_;
int return_mode_;
int degree_mode_;
int rpm_;
bool info_print_flag_;
std::string calibration_file_;
float scan_start_angle_;
float scan_end_angle_;
int packet_end;
double point_time;
double packet_end_time;
bool coordinate_opt;
bool echo_second_;
bool config_vert_;
bool config_vert_angle;
bool print_vert_;
bool config_vert_file_;
double vert_angle;
double cos_scan_altitude_caliration[LSC16_SCANS_PER_FIRING];
double sin_scan_altitude_caliration[LSC16_SCANS_PER_FIRING];
};
float sin_azimuth_table[ROTATION_MAX_UNITS];
float cos_azimuth_table[ROTATION_MAX_UNITS];
float VERT_ANGLE[32];
float HORI_ANGLE[32];
float aIntensityCal[7][32];
float aIntensityCal_old[1600][32];
bool Curvesis_new = true;
int g_ChannelNum[32][51];
float CurvesRate[32];
float temper = 31.0;
int tempPacketNum = 0;
int numOfLasers = 16;
int TEMPERATURE_RANGE = 40;
} // namespace lslidar_rawdata
POINT_CLOUD_REGISTER_POINT_STRUCT(lslidar_rawdata::PointXYZIRT,
(float, x, x)(float, y, y)(float, z, z)
(float, intensity, intensity)
(uint16_t, ring, ring)
(double, time, time)
)
#endif // __RAWDATA_H
sensors/lslidar_C16/lslidar_c16_decoder/launch/lslidar_c16.launch
+51
View File
@@ -0,0 +1,51 @@
<launch>
<arg name="device_ip" default="192.168.10.6" />
<arg name="msop_port" default="2368" />
<arg name="difop_port" default="2369" />
<arg name="return_mode" default="1" />
<arg name="time_synchronization" default="false" />
<node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver_node" output="screen">
<!--param name="pcap" value="$(find lslidar_c16_driver)/pcap/xxx.pcap" /-->
<param name="device_ip" value="$(arg device_ip)" />
<param name="msop_port" value="$(arg msop_port)" />
<param name="difop_port" value="$(arg difop_port)"/>
<!-- <param name="frame_id" value="velodyne"/> -->
<param name="frame_id" value="lidar"/>
<param name="add_multicast" value="false"/>
<param name="group_ip" value="224.1.1.2"/>
<param name="rpm" value="600"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
</node>
<node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen">
<param name="scan_frame_id" value="laser_link"/>
<param name="min_range" value="0.15"/>
<param name="max_range" value="150.0"/>
<param name="frequency" value="10"/>
<param name="config_vert" value="true"/>
<param name="print_vert" value="false"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="degree_mode" value="2"/>
<param name="config_vert_file" value="false"/>
<param name="distance_unit" value="0.25"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
<param name="scan_start_angle" value="0.0"/>
<param name="scan_end_angle" value="36000.0"/>
<param name="scan_num" value="8"/>
<param name="publish_scan" value="false"/>
<remap from="/scan_topic" to="scan"/>
<param name="echo_second" value="false"/>
<param name="pointcloud_topic" value="lslidar_point_cloud"/>
<param name="coordinate_opt" value="true"/>
</node>
<!-- <node pkg="adaptive_clustering" type="adaptive_clustering" name="adaptive_clustering_node" output="screen">
</node> -->
<!-- <node pkg="rviz" type="rviz" name="rviz" args="-d $(find lslidar_c16_decoder)/../../rviz_cfg/EcoCar.rviz" /> -->
<!--node pkg="tf" type="static_transform_publisher" name="laser_link_to_world" args="0 0 0 0 0 0 world laser_link 100" /-->
</launch>
sensors/lslidar_C16/lslidar_c16_decoder/launch/lslidar_c16_double.launch
+89
View File
@@ -0,0 +1,89 @@
<launch>
<arg name="device_ip1" default="192.168.1.200" />
<arg name="device_ip2" default="192.168.1.201" />
<arg name="msop_port1" default="2368" />
<arg name="difop_port1" default="2369" />
<arg name="msop_port2" default="2370" />
<arg name="difop_port2" default="2371" />
<arg name="return_mode" default="1" />
<arg name="time_synchronization" default="false" />
<node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver_node" output="screen" ns="LeftLidar" >
<param name="lidar_ip" value="$(arg device_ip1)" />
<param name="msop_port" value="$(arg msop_port1)" />
<param name="difop_port" value="$(arg difop_port1)"/>
<param name="frame_id" value="laser_link_left"/>
<param name="add_multicast" value="false"/>
<param name="group_ip" value="224.1.1.2"/>
<param name="rpm" value="600"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
</node>
<node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen" ns="LeftLidar">
<param name="scan_frame_id" value="laser_link"/>
<param name="min_range" value="0.15"/>
<param name="max_range" value="150.0"/>
<param name="frequency" value="10"/>
<param name="config_vert" value="true"/>
<param name="print_vert" value="false"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="degree_mode" value="2"/>
<param name="config_vert_file" value="false"/>
<param name="distance_unit" value="0.25"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
<param name="scan_start_angle" value="0.0"/>
<param name="scan_end_angle" value="36000.0"/>
<param name="scan_num" value="8"/>
<param name="publish_scan" value="true"/>
<remap from="/scan_topic" to="scan"/>
<param name="echo_second" value="false"/>
<param name="pointcloud_topic" value="lslidar_point_cloud">
<param name="coordinate_opt" value="true">
</node>
<node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver_node" output="screen" ns="RightLidar" >
<param name="lidar_ip" value="$(arg device_ip2)" />
<param name="msop_port" value="$(arg msop_port2)" />
<param name="difop_port" value="$(arg difop_port2)"/>
<param name="frame_id" value="laser_link_right"/>
<param name="add_multicast" value="false"/>
<param name="group_ip" value="224.1.1.2"/>
<param name="rpm" value="600"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
</node>
<node pkg="lslidar_c16_decoder" type="lslidar_c16_decoder_node" name="lslidar_c16_decoder_node" output="screen" ns="RightLidar">
<param name="scan_frame_id" value="laser_link"/>
<param name="min_range" value="0.15"/>
<param name="max_range" value="150.0"/>
<param name="frequency" value="10"/>
<param name="config_vert" value="true"/>
<param name="print_vert" value="false"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="degree_mode" value="2"/>
<param name="config_vert_file" value="false"/>
<param name="distance_unit" value="0.25"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
<param name="scan_start_angle" value="0.0"/>
<param name="scan_end_angle" value="36000.0"/>
<param name="scan_num" value="8"/>
<param name="publish_scan" value="true"/>
<remap from="/scan_topic" to="scan"/>
<param name="echo_second" value="false"/>
<param name="pointcloud_topic" value="lslidar_point_cloud">
<param name="coordinate_opt" value="true">
</node>
<!--node pkg="tf" type="static_transform_publisher" name="laser_link_right_to_world" args="0 0 0 0 0 0 world laser_link_right 100" /-->
<!--node pkg="tf" type="static_transform_publisher" name="laser_link_left_to_world" args="0 0 0 1 0 0 world laser_link_left 100" /-->
</launch>
sensors/lslidar_C16/lslidar_c16_decoder/launch/lslidar_c16_nodelet.launch
+59
View File
@@ -0,0 +1,59 @@
<launch>
<arg name="child_frame_id" default="lslidar"/>
<arg name="topic_packet" default="/apollo/sensor/lslidar_c16/LslidarPacket"/>
<arg name="topic_pointcloud" default="/apollo/sensor/lslidar_c16/PointCloud2"/>
<arg name="device_ip" default="192.168.1.200" />
<arg name="msop_port" default="2368" />
<arg name="difop_port" default="2369" />
<arg name="return_mode" default="1" />
<arg name="time_synchronization" default="true" />
<arg name="manager_name" default="nodelet_manager"/>
<!--include file="$(find lslidar_c16_driver)/launch/lslidar_c16_driver_nodelet.launch"/-->
<!-- nodelet manager -->
<node pkg="nodelet" type="nodelet" name="nodelet_manager" args="manager" output="screen" />
<node pkg="nodelet" type="nodelet"
name="lslidar_c16_driver_nodelet"
args="load lslidar_c16_driver/DriverNodelet nodelet_manager" respawn="true">
<param name="device_ip" value="$(arg device_ip)" />
<param name="msop_port" value="$(arg msop_port)" />
<param name="difop_port" value="$(arg difop_port)"/>
<param name="frame_id" value="laser_link"/>
<param name="add_multicast" value="false"/>
<param name="group_ip" value="224.1.1.2"/>
<param name="rpm" value="600"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
</node>
<node pkg="nodelet" type="nodelet"
name="LslidarC16DecoderNodelet"
args="load lslidar_c16_decoder/LslidarC16DecoderNodelet
$(arg manager_name)"
output="screen" respawn="true">
<param name="scan_frame_id" value="laser_link"/>
<param name="min_range" value="0.15"/>
<param name="max_range" value="150.0"/>
<param name="config_vert" value="true"/>
<param name="print_vert" value="false"/>
<param name="return_mode" value="$(arg return_mode)"/>
<param name="degree_mode" value="2"/>
<param name="config_vert_file" value="false"/>
<param name="distance_unit" value="0.25"/>
<param name="time_synchronization" value="$(arg time_synchronization)"/>
<param name="scan_start_angle" value="0.0"/>
<param name="scan_end_angle" value="36000.0"/>
<param name="scan_num" value="8"/>
<param name="publish_scan" value="true"/>
</node>
<!-- <node name="rviz" pkg="rviz" type="rviz" args="-d $(find lslidar_c16_decoder)/launch/lslidar_c16.rviz" output="screen"/>
-->
</launch>
sensors/lslidar_C16/lslidar_c16_decoder/nodelets.xml
+12
View File
@@ -0,0 +1,12 @@
<library path="lib/liblslidar_c16_decoder_nodelet">
<class name="lslidar_c16_decoder/LslidarC16DecoderNodelet"
type="lslidar_c16_decoder::LslidarC16DecoderNodelet"
base_class_type="nodelet::Nodelet">
<description>
Aggregates points from multiple packets, publishing PointCloud2.
</description>
</class>
</library>
sensors/lslidar_C16/lslidar_c16_decoder/package.xml
+48
View File
@@ -0,0 +1,48 @@
<package>
<name>lslidar_c16_decoder</name>
<version>1.0.0</version>
<description>
Point cloud conversions for Leishen C16 LIDARs.
</description>
<url>http://www.ros.org/wiki/lslidar_c16</url>
<maintainer email="yutong@lslidar.com">Yutong</maintainer>
<author>Yutong</author>
<license>GNC General Public License V3.0</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>angles</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>lslidar_c16_driver</build_depend>
<build_depend>lslidar_c16_msgs</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>yaml-cpp</build_depend>
<!-- these build dependencies are only needed for unit testing -->
<build_depend>tf2_ros</build_depend>
<run_depend>angles</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>pcl_conversions</run_depend>
<!-- <run_depend>python-yaml</run_depend> -->
<run_depend>roscpp</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>lslidar_c16_driver</run_depend>
<run_depend>lslidar_c16_msgs</run_depend>
<!-- <run_depend>yaml-cpp</run_depend> -->
<run_depend>dynamic_reconfigure</run_depend>
<export>
<nodelet plugin="${prefix}/nodelets.xml"/>
</export>
</package>
sensors/lslidar_C16/lslidar_c16_decoder/params/lslidar_c16_db.yaml
+18
View File
@@ -0,0 +1,18 @@
lasers:
- {vert_correction: -15} #Channel 0
- {vert_correction: 1} #Channel 1
- {vert_correction: -13} #Channel 2
- {vert_correction: 3} #Channel 3
- {vert_correction: -11} #Channel 4
- {vert_correction: 5} #Channel 5
- {vert_correction: -9} #Channel 6
- {vert_correction: 7} #Channel 7
- {vert_correction: -7} #Channel 8
- {vert_correction: 9} #Channel 9
- {vert_correction: -5} #Channel 10
- {vert_correction: 11} #Channel 11
- {vert_correction: -3} #Channel 12
- {vert_correction: 13} #Channel 13
- {vert_correction: -1} #Channel 14
- {vert_correction: 15} #Channel 15
sensors/lslidar_C16/lslidar_c16_decoder/rviz_cfg/c16.rviz
+152
View File
@@ -0,0 +1,152 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
Splitter Ratio: 0.5
Tree Height: 565
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
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: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 255
Min Color: 0; 0; 0
Min Intensity: 9
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.00999999978
Style: Flat Squares
Topic: /lslidar_point_cloud
Unreliable: false
Use Fixed Frame: true
Use rainbow: true
Value: true
- Class: rviz/Axes
Enabled: true
Length: 1
Name: Axes
Radius: 0.100000001
Reference Frame: <Fixed Frame>
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: laser_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 21.8094082
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 1.78493285
Y: 1.5556097
Z: 0.881267607
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.509797454
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.85353827
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 846
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1200
X: 60
Y: 26
sensors/lslidar_C16/lslidar_c16_decoder/src/cloud_node.cc
+34
View File
@@ -0,0 +1,34 @@
/*
* This file is part of lslidar_n301 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "lslidar_c16_decoder/convert.h"
/** Main node entry point. */
int main(int argc, char** argv)
{
ros::init(argc, argv, "lslidar_decoder_node");
ros::NodeHandle node;
ros::NodeHandle priv_nh("~");
// create conversion class, which subscribes to raw data
lslidar_c16_decoder::Convert conv(node, priv_nh);
// handle callbacks until shut down
ros::spin();
return 0;
}
sensors/lslidar_C16/lslidar_c16_decoder/src/cloud_nodelet.cc
+50
View File
@@ -0,0 +1,50 @@
/*
* This file is part of lslidar_n301 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include "lslidar_c16_decoder/convert.h"
namespace lslidar_c16_decoder
{
class LslidarC16DecoderNodelet : public nodelet::Nodelet
{
public:
LslidarC16DecoderNodelet()
{
}
~LslidarC16DecoderNodelet()
{
}
private:
virtual void onInit();
boost::shared_ptr<Convert> conv_;
};
/** @brief Nodelet initialization. */
void LslidarC16DecoderNodelet::onInit()
{
conv_.reset(new Convert(getNodeHandle(), getPrivateNodeHandle()));
}
} // namespace lslidar_c16_decoder
// parameters: class type, base class type
PLUGINLIB_EXPORT_CLASS(lslidar_c16_decoder::LslidarC16DecoderNodelet, nodelet::Nodelet);
sensors/lslidar_C16/lslidar_c16_decoder/src/convert.cc
+201
View File
@@ -0,0 +1,201 @@
/*
* This file is part of lslidar_n301 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "lslidar_c16_decoder/convert.h"
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/filter.h>
namespace lslidar_c16_decoder {
std::string model;
/** @brief Constructor. */
Convert::Convert(ros::NodeHandle node, ros::NodeHandle private_nh) : data_(new lslidar_rawdata::RawData()) {
scan_nums = 0;
scan_start = true;
data_->loadConfigFile(node, private_nh); // load lidar parameters
private_nh.param("model", model, std::string("LSC16"));
// advertise output point cloud (before subscribing to input data)
private_nh.param("pointcloud_topic", pointcloud_topic, std::string("lslidar_point_cloud"));
output_ = node.advertise<sensor_msgs::PointCloud2>(pointcloud_topic, 10);
scan_pub = node.advertise<sensor_msgs::LaserScan>("scan_topic", 100);
// subscribe to lslidar packets
packet_sub_ = node.subscribe("lslidar_packet_c16", 10, &Convert::processScan, (Convert *) this,
ros::TransportHints().tcpNoDelay(true));
private_nh.param("time_synchronization", time_synchronization_, false);
private_nh.param("scan_num", scan_num, 8);
private_nh.param("publish_scan", publish_scan, false);
private_nh.param("scan_frame_id", scan_frame_id, std::string("laser_link"));
private_nh.param("frequency", frequency, 10);
if(publish_scan) {
if (scan_num < 0) {
scan_num = 0;
ROS_WARN("channel_num_ outside of the index, select channel 0 instead!");
} else if (scan_num > 15) {
scan_num = 15;
ROS_WARN("channel_num_ outside of the index, select channel 15 instead!");
}
ROS_INFO("select scan_num: %d", scan_num);
}
switch (frequency){
case 5:
horizontal_angle_resolution = DEG2RAD(0.09);
break;
case 20:
horizontal_angle_resolution = DEG2RAD(0.36);
break;
default:
horizontal_angle_resolution = DEG2RAD(0.18);
}
if (time_synchronization_) {
sync_sub_ = node.subscribe("sync_header", 10, &Convert::timeSync, (Convert *) this,
ros::TransportHints().tcpNoDelay(true));
}
}
void Convert::timeSync(const sensor_msgs::TimeReferenceConstPtr &time_msg) {
global_time = time_msg->header.stamp;
}
void Convert::removeNan(lslidar_rawdata::VPointCloud &cloud_in, lslidar_rawdata::VPointCloud &cloud_out,
std::vector<int> &index) {
if (&cloud_in != &cloud_out) {
cloud_out.header = cloud_in.header;
cloud_out.resize(cloud_in.size());
cloud_out.sensor_origin_ = cloud_in.sensor_origin_;
cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
}
index.resize(cloud_in.size());
if (cloud_in.is_dense) {
cloud_out = cloud_in;
for (std::size_t j = 0; j < cloud_out.size(); ++j)
index[j] = j;
} else {
std::size_t j = 0;
for (std::size_t i = 0; i < cloud_in.size(); ++i) {
if (!std::isfinite(cloud_in[i].x) ||
!std::isfinite(cloud_in[i].y) ||
!std::isfinite(cloud_in[i].z))
continue;
cloud_out[j] = cloud_in[i];
index[j] = i;
j++;
}
if(j != cloud_in.size()){
cloud_out.resize(j);
index.resize(j);
}
cloud_out.height = 1;
cloud_out.width = static_cast<std::uint32_t>(j);
cloud_out.is_dense =true;
}
}
void Convert::publishScan(lslidar_c16_msgs::LslidarC16SweepPtr &sweepData, int scanNum) {
sensor_msgs::LaserScan::Ptr scan(new sensor_msgs::LaserScan);
if (sweepData->scans[scanNum].points.size() <= 1)
return;
uint16_t point_num = 2 * M_PI / horizontal_angle_resolution;
scan->header.frame_id = scan_frame_id;
scan->header.stamp = sweepData->header.stamp; // timestamp will obtained from sweep data stamp
scan->angle_min = 0;
scan->angle_max = 2 * M_PI;
scan->angle_increment = horizontal_angle_resolution;
scan->range_min = 0.15;
scan->range_max = 150;
scan->ranges.reserve(point_num);
scan->ranges.assign(point_num, std::numeric_limits<float>::infinity());
scan->intensities.reserve(point_num);
scan->intensities.assign(point_num, std::numeric_limits<float>::infinity());
for (uint16_t i = 0; i < sweepData->scans[scanNum].points.size() - 1; i++) {
int point_idx = sweepData->scans[scanNum].points[i].azimuth / scan->angle_increment;
if (point_idx >= point_num)
point_idx = 0;
if (point_idx < 0)
point_idx = point_num - 1;
scan->ranges[point_num - 1 - point_idx] = sweepData->scans[scanNum].points[i].distance;
scan->intensities[point_num - 1 - point_idx] = sweepData->scans[scanNum].points[i].intensity;
}
scan_pub.publish(scan);
}
/** @brief Callback for raw scan messages. */
void Convert::processScan(const lslidar_c16_msgs::LslidarC16ScanUnified::ConstPtr &scanMsg) {
// pcl::PointCloud<pcl::PointXYZI>::Ptr outPoints(new pcl::PointCloud<pcl::PointXYZI>);
lslidar_rawdata::VPointCloud::Ptr outPoints(new lslidar_rawdata::VPointCloud);
sweep_data = lslidar_c16_msgs::LslidarC16SweepPtr(new lslidar_c16_msgs::LslidarC16Sweep());
if (time_synchronization_) {
outPoints->header.stamp = pcl_conversions::toPCL(scanMsg->header).stamp;
scan_timestamp = scanMsg->header.stamp;
} else {
outPoints->header.stamp = ros::Time::now().toNSec() / 1000ull;
}
outPoints->header.frame_id = scanMsg->header.frame_id;
outPoints->clear();
outPoints->height = 16;
outPoints->width = 24 * (int) scanMsg->packets.size();
outPoints->is_dense = false;
outPoints->resize(outPoints->height * outPoints->width);
int blockNum, dsr;
lslidar_rawdata::VPoint point;
point.x = NAN;
point.y = NAN;
point.z = NAN;
point.intensity = 0;
for (int i = 0; i < outPoints->height * outPoints->width; ++i) {
blockNum = i / 16;
dsr = i % 16;
outPoints->at(blockNum, dsr) = point;
}
// process each packet provided by the driver
data_->block_num = 0;
for (int i = 0; i < scanMsg->packets.size(); ++i) {
data_->unpack(scanMsg->packets[i], outPoints, i, sweep_data);
}
removeNan(*outPoints, *outPoints, indices);
sensor_msgs::PointCloud2 outMsg;
pcl::toROSMsg(*outPoints, outMsg);
output_.publish(outMsg);
if (publish_scan) { publishScan(sweep_data, scan_num); }
}
} // namespace lslidar_c16_decoder
sensors/lslidar_C16/lslidar_c16_decoder/src/rawdata.cc
+398
View File
@@ -0,0 +1,398 @@
/*
* This file is part of lslidar_n301 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "lslidar_c16_decoder/rawdata.h"
#include <angles/angles.h>
#include <yaml-cpp/yaml.h>
namespace lslidar_rawdata {
RawData::RawData() {
this->is_init_angle_ = false;
this->is_init_curve_ = false;
this->is_init_top_fw_ = false;
}
void RawData::loadConfigFile(ros::NodeHandle node, ros::NodeHandle private_nh) {
std::string model;
std::string resolution_param;
private_nh.param("start_angle", start_angle_, float(0));
private_nh.param("end_angle", end_angle_, float(360));
private_nh.param("distance_unit", distance_unit_, float(0.25));
private_nh.param("calibration_file", calibration_file_, std::string(""));
private_nh.param("scan_start_angle", scan_start_angle_, float(0.0));
private_nh.param("scan_end_angle", scan_end_angle_, float(36000.0));
private_nh.param("rpm", rpm_, 300);
if (start_angle_ < 0 || start_angle_ > 360 || end_angle_ < 0 || end_angle_ > 360) {
start_angle_ = 0;
end_angle_ = 360;
ROS_INFO_STREAM("start angle and end angle select feature deactivated.");
} else {
ROS_INFO_STREAM("start angle and end angle select feature activated.");
}
angle_flag_ = true;
if (start_angle_ > end_angle_) {
angle_flag_ = false;
ROS_INFO_STREAM("Start angle is smaller than end angle, not the normal state!");
}
ROS_INFO_STREAM(
"start_angle: " << start_angle_ << " end_angle: " << end_angle_ << " angle_flag: " << angle_flag_);
start_angle_ = start_angle_ / 180 * M_PI;
end_angle_ = end_angle_ / 180 * M_PI;
private_nh.param("max_range", max_distance_, 150.0f);
private_nh.param("min_range", min_distance_, 0.15f);
private_nh.param("return_mode", return_mode_, 1);
private_nh.param("config_vert", config_vert_, true);
private_nh.param("print_vert", print_vert_, true);
private_nh.param("config_vert_file", config_vert_file_, false);
private_nh.param("degree_mode", degree_mode_, 1);
private_nh.param("echo_second", echo_second_, false);
private_nh.param("coordinate_opt", coordinate_opt, true);
ROS_INFO_STREAM("distance threshlod, max: " << max_distance_ << ", min: " << min_distance_);
ROS_INFO_STREAM("return mode : " << return_mode_);
if (2 == degree_mode_) {
ROS_INFO_STREAM("vertical angle resolution: 2 degree");
} else if (1 == degree_mode_) {
ROS_INFO_STREAM("vertical angle resolution: 1.33 degree");
}
intensity_mode_ = 1;
info_print_flag_ = false;
config_vert_angle = false;
private_nh.param("model", model, std::string("LSC16"));
numOfLasers = 16;
R1_ = 0.04319; //calibration
R2_ = 0.010875;
intensityFactor = 51;
//return mode default
//return_mode_ = 1;
if (2 == degree_mode_) {
//Vertical Angle Calibration for device package
for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
cos_scan_altitude_caliration[i] = std::cos(scan_altitude_original_2[i]);
sin_scan_altitude_caliration[i] = std::sin(scan_altitude_original_2[i]);
scan_altitude[i] = scan_altitude_original_2[i];
}
} else if (1 == degree_mode_) {
for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
cos_scan_altitude_caliration[i] = std::cos(scan_altitude_original_1[i]);
sin_scan_altitude_caliration[i] = std::sin(scan_altitude_original_1[i]);
scan_altitude[i] = scan_altitude_original_1[i];
}
}
#if 0
//Vertical Angle Calibration for file
if(config_vert_file_){
YAML::Node calibration_config = YAML::LoadFile(calibration_file_);
for(int i = 0; i < LSC16_SCANS_PER_FIRING; i++){
vert_angle = calibration_config["lasers"][i]["vert_correction"].as<double>();
cos_scan_altitude_caliration[i] = std::cos(vert_angle*DEG_TO_RAD);
sin_scan_altitude_caliration[i] = std::sin(vert_angle*DEG_TO_RAD);
}
}
#endif
for (uint16_t rot_index = 0; rot_index < ROTATION_MAX_UNITS; ++rot_index) {
float rotation = angles::from_degrees(ROTATION_RESOLUTION * rot_index);
cos_azimuth_table[rot_index] = cosf(rotation);
sin_azimuth_table[rot_index] = sinf(rotation);
}
// receive difop data
// subscribe to difop lslidar packets, if not right correct data in difop, it will not revise the correct data in the
// VERT_ANGLE, HORI_ANGLE etc.
difop_sub_ = node.subscribe("lslidar_packet_difop_c16", 10, &RawData::processDifop, (RawData *) this);
}
void RawData::processDifop(const lslidar_c16_msgs::LslidarC16Packet::ConstPtr &difop_msg) {
// std::cout << "Enter difop callback!" << std::endl;
const uint8_t *data = &difop_msg->data[0];
bool is_support_dual_return = false;
// check header
if (data[0] != 0xa5 || data[1] != 0xff || data[2] != 0x00 || data[3] != 0x5a) {
return;
}
int version_data = data[1202];
if (config_vert_) {
if (2 == version_data) {
for (int i = 0; i < 16; i++) {
uint8_t data1 = data[234 + 2 * i];
uint8_t data2 = data[234 + 2 * i + 1];
int vert_angle = data1 * 256 + data2;
if (vert_angle > 32767) {
vert_angle = vert_angle - 65535;
}
//ROS_INFO_STREAM("data vert: " << vert_angle);
scan_altitude[i] = ((float) vert_angle / 100.f) * DEG_TO_RAD;
if (2 == degree_mode_) {
if (scan_altitude[i] != 0) {
if (fabs(scan_altitude_original_2[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
scan_altitude[i] = scan_altitude_original_2[i];
}
} else {
scan_altitude[i] = scan_altitude_original_2[i];
}
} else if (1 == degree_mode_) {
if (scan_altitude[i] != 0) {
if (fabs(scan_altitude_original_1[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
scan_altitude[i] = scan_altitude_original_1[i];
}
} else {
scan_altitude[i] = scan_altitude_original_1[i];
}
}
config_vert_angle = true;
}
} else {
for (int i = 0; i < 16; i++) {
uint8_t data1 = data[245 + 2 * i];
uint8_t data2 = data[245 + 2 * i + 1];
int vert_angle = data1 * 256 + data2;
if (vert_angle > 32767) {
vert_angle = vert_angle - 65535;
}
//ROS_INFO_STREAM("data vert: " << vert_angle);
scan_altitude[i] = ((float) vert_angle / 100.f) * DEG_TO_RAD;
if (2 == degree_mode_) {
if (scan_altitude[i] != 0) {
if (fabs(scan_altitude_original_2[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
scan_altitude[i] = scan_altitude_original_2[i];
}
} else {
scan_altitude[i] = scan_altitude_original_2[i];
}
} else if (1 == degree_mode_) {
if (scan_altitude[i] != 0) {
if (fabs(scan_altitude_original_1[i] - scan_altitude[i]) * RAD_TO_DEG > 1.5) {
scan_altitude[i] = scan_altitude_original_1[i];
}
} else {
scan_altitude[i] = scan_altitude_original_1[i];
}
}
config_vert_angle = true;
}
}
}
// rpm
if ((data[8] == 0x04) && (data[9] == 0xB0)) {
rpm_ = 1200;
} else if ((data[8] == 0x02) && (data[9] == 0x58)) {
rpm_ = 600;
} else if ((data[8] == 0x01) && (data[9] == 0x2C)) {
rpm_ = 300;
} else if ((data[8] == 0x00) && (data[9] == 0x2f)) {
rpm_ = 600;
} else {
//rpm_ = 300;
//ROS_WARN("Invalid motor rpm!");
}
if (print_vert_) {
//ROS_INFO("rpm is %d", rpm_);
}
}
/** @brief convert raw packet to point cloud
*
* @param pkt raw packet to unpack
* @param pc shared pointer to point cloud (points are appended)
*/
void
RawData::unpack(const lslidar_c16_msgs::LslidarC16Packet &pkt, VPointCloud::Ptr pointcloud,
int Packet_num,
lslidar_c16_msgs::LslidarC16SweepPtr &sweep_data) {
float azimuth; // 0.01 dgree
float intensity;
float azimuth_diff;
float azimuth_corrected_f;
int azimuth_corrected;
if (config_vert_angle) {
for (int i = 0; i < LSC16_SCANS_PER_FIRING; i++) {
cos_scan_altitude_caliration[i] = std::cos(scan_altitude[i]);
sin_scan_altitude_caliration[i] = std::sin(scan_altitude[i]);
if (print_vert_) {
ROS_INFO("Channel %d Data %f", i, scan_altitude[i] * RAD_TO_DEG);
}
}
config_vert_angle = false;
}
const raw_packet_t *raw = (const raw_packet_t *) &pkt.data[0];
packet_end_time = pkt.stamp.toSec();
for (int block = 0; block < BLOCKS_PER_PACKET; block++, this->block_num++) // 1 packet:12 data blocks
{
if (UPPER_BANK != raw->blocks[block].header) {
//ROS_INFO_STREAM_THROTTLE(180, "skipping LSLIDAR DIFOP packet");
break;
}
azimuth = (float) (256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1);
if (2 == return_mode_) {
if (block < (BLOCKS_PER_PACKET - 2)) // 12
{
int azi1, azi2;
azi1 = 256 * raw->blocks[block + 2].rotation_2 + raw->blocks[block + 2].rotation_1;
azi2 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
} else {
int azi1, azi2;
azi1 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
azi2 = 256 * raw->blocks[block - 2].rotation_2 + raw->blocks[block - 2].rotation_1;
azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
}
} else {
if (block < (BLOCKS_PER_PACKET - 1)) // 12
{
int azi1, azi2;
azi1 = 256 * raw->blocks[block + 1].rotation_2 + raw->blocks[block + 1].rotation_1;
azi2 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
} else {
//block 12
int azi1, azi2;
azi1 = 256 * raw->blocks[block].rotation_2 + raw->blocks[block].rotation_1;
azi2 = 256 * raw->blocks[block - 1].rotation_2 + raw->blocks[block - 1].rotation_1;
azimuth_diff = (float) ((36000 + azi1 - azi2) % 36000);
}
}
float cos_azimuth;
float sin_azimuth;
packet_end = (POINTS_ONE_CHANNEL_PER_SECOND / BLOCKS_ONE_CHANNEL_PER_PKT) / 2 * 60 / rpm_ * return_mode_;
for (int firing = 0, k = 0; firing < LSC16_FIRINGS_PER_BLOCK; firing++) // 2
{
for (int dsr = 0; dsr < LSC16_SCANS_PER_FIRING; dsr++, k += RAW_SCAN_SIZE) // 16 3
{
azimuth_corrected_f = azimuth + azimuth_diff / (LSC16_SCANS_PER_FIRING * 2) *
(LSC16_SCANS_PER_FIRING * firing + dsr);
azimuth_corrected = ((int) round(azimuth_corrected_f)) % 36000; // convert to integral value...
cos_azimuth = cos_azimuth_table[azimuth_corrected];
sin_azimuth = sin_azimuth_table[azimuth_corrected];
//distance
union two_bytes tmp;
tmp.bytes[0] = raw->blocks[block].data[k];
tmp.bytes[1] = raw->blocks[block].data[k + 1];
int distance = tmp.uint;
// read intensity
intensity = raw->blocks[block].data[k + 2];
float distance2 = (distance * DISTANCE_RESOLUTION) * distance_unit_;
//The offset calibration
float arg_horiz = (float) azimuth_corrected_f * ROTATION_RESOLUTION;
arg_horiz = arg_horiz > 360 ? (arg_horiz - 360) : arg_horiz;
float arg_horiz_orginal = (arg_horiz - 14.68) * M_PI / 180;
if (2 == return_mode_) {
point_time = packet_end_time - 100 * (5 - block / 2) * 1e-6 -
3.125 * (31 - firing * 16 - dsr) * 1e-6;
} else {
point_time =
packet_end_time - 100 * (11 - block) - 1e-6 - 3.125 * (31 - firing * 16 - dsr) * 1e-6;
}
VPoint point;
if ((distance2 > max_distance_) || (distance2 < min_distance_) ||
(azimuth_corrected_f < scan_start_angle_) || (azimuth_corrected_f > scan_end_angle_) ||
(Packet_num < 10 && azimuth_corrected > 18000) ||
(Packet_num > int(packet_end * 0.9) && azimuth_corrected < 18000) ||
(2 == return_mode_ && echo_second_ && block % 2 == 0)) {
point.x = NAN;
point.y = NAN;
point.z = NAN;
point.intensity = 0;
point.ring = (dsr % 2) * 8 + dsr / 2;
point.time = point_time;
pointcloud->at(2 * this->block_num + firing, point.ring) = point;
} else {
if (coordinate_opt) {
point.x = distance2 * cos_scan_altitude_caliration[dsr] * cos_azimuth +
R1_ * cos((14.68 - arg_horiz) * M_PI / 180);
point.y = -distance2 * cos_scan_altitude_caliration[dsr] * sin_azimuth -
R1_ * sin((14.68 - arg_horiz) * M_PI / 180);
point.z = distance2 * sin_scan_altitude_caliration[dsr] + R1_;
} else {
point.x = distance2 * cos_scan_altitude_caliration[dsr] * sin_azimuth +
R1_ * sin(arg_horiz_orginal);
point.y = distance2 * cos_scan_altitude_caliration[dsr] * cos_azimuth +
R1_ * cos(arg_horiz_orginal);
point.z = distance2 * sin_scan_altitude_caliration[dsr] + R1_;
}
point.intensity = intensity;
point.ring = (dsr % 2) * 8 + dsr / 2;
point.time = point_time;
pointcloud->at(2 * this->block_num + firing, point.ring) = point;
sweep_data->scans[dsr].points.push_back(lslidar_c16_msgs::LslidarC16Point());
lslidar_c16_msgs::LslidarC16Point &new_point = sweep_data->scans[dsr].points[
sweep_data->scans[dsr].points.size() - 1];
// Pack the data into point msg
new_point.x = point.x;
new_point.y = point.y;
new_point.z = point.z;
new_point.azimuth = angles::from_degrees(azimuth_corrected * ROTATION_RESOLUTION);
new_point.distance = sqrt(pow(point.x, 2) + pow(point.y, 2) + pow(point.z, 2));
new_point.intensity = point.intensity;
}
}
}
}
}
} // namespace lslidar_c16_decoder
sensors/lslidar_C16/lslidar_c16_driver/CMakeLists.txt
+94
View File
@@ -0,0 +1,94 @@
cmake_minimum_required(VERSION 2.8.3)
project(lslidar_c16_driver)
add_compile_options(-std=c++14)
set(CMAKE_BUILD_TYPE Release)#RelWithDebInfo
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
set(${PROJECT_NAME}_CATKIN_DEPS
angles
pcl_ros
roscpp
sensor_msgs
tf
dynamic_reconfigure
lslidar_c16_msgs
nodelet
)
set(libpcap_LIBRARIES -lpcap)
find_package(catkin REQUIRED COMPONENTS
${${PROJECT_NAME}_CATKIN_DEPS}
pcl_conversions
rospy
std_msgs
genmsg
cv_bridge
message_generation
)
find_package(Boost COMPONENTS signals)
find_package(Boost REQUIRED COMPONENTS thread)
find_package(PkgConfig REQUIRED)
include_directories(
include
${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS}
${dynamic_reconfigure_PACKAGE_PATH}/cmake/cfgbuild.cmake)
catkin_package(
CATKIN_DEPENDS ${${PROJECT_NAME}_CATKIN_DEPS}
CATKIN_DEPENDS message_runtime std_msgs
)
add_library(lslidar_input_c16 src/input.cc)
target_link_libraries(lslidar_input_c16
${catkin_LIBRARIES}
${libpcap_LIBRARIES})
add_library(lslidar_c16_driver src/lslidar_c16_driver.cpp)
target_link_libraries(lslidar_c16_driver
lslidar_input_c16
${catkin_LIBRARIES})
# build the nodelet version
add_library(lslidar_c16_driver_nodelet src/lslidar_c16_driver_nodelet.cc src/lslidar_c16_driver.cpp)
target_link_libraries(lslidar_c16_driver_nodelet
lslidar_input_c16
${catkin_LIBRARIES}
)
add_executable(lslidar_c16_driver_node src/lslidar_c16_driver_node.cpp)
if(catkin_EXPORTED_TARGETS)
add_dependencies(lslidar_input_c16 ${catkin_EXPORTED_TARGETS})
endif()
target_link_libraries(lslidar_c16_driver_node
lslidar_c16_driver
lslidar_input_c16
${catkin_LIBRARIES}
${libpcap_LIBRARIES}
)
install(TARGETS lslidar_input_c16 lslidar_c16_driver lslidar_c16_driver_nodelet lslidar_c16_driver_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
install(FILES
nodelet_lslidar_c16.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
sensors/lslidar_C16/lslidar_c16_driver/include/lslidar_c16_driver/input.h
+130
View File
@@ -0,0 +1,130 @@
/*
* This file is part of lslidar_c16 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*
* Input -- base class used to access the data independently of
* its source
*
* InputSocket -- derived class reads live data from the device
* via a UDP socket
*
* InputPCAP -- derived class provides a similar interface from a
* PCAP dump
*/
#ifndef __LSLIDAR_INPUT_H_
#define __LSLIDAR_INPUT_H_
#include <unistd.h>
#include <stdio.h>
#include <pcap.h>
#include <netinet/in.h>
#include <ros/ros.h>
#include <lslidar_c16_msgs/LslidarC16Packet.h>
#include <string>
#include <sstream>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <poll.h>
#include <errno.h>
#include <fcntl.h>
#include <sys/file.h>
#include <signal.h>
#include <sensor_msgs/TimeReference.h>
namespace lslidar_c16_driver {
static uint16_t MSOP_DATA_PORT_NUMBER = 2368; // lslidar default data port on PC
static uint16_t DIFOP_DATA_PORT_NUMBER = 2369; // lslidar default difop data port on PC
/**
* 从在线的网络数据或离线的网络抓包数据（pcap文件）中提取出lidar的原始数据，即packet数据包
* @brief The Input class,
*
* @param private_nh 一个NodeHandled,用于通过节点传递参数
* @param port
* @returns 0 if successful,
* -1 if end of file
* >0 if incomplete packet (is this possible?)
*/
class Input {
public:
Input(ros::NodeHandle private_nh, uint16_t port);
virtual ~Input() {
}
virtual int getPacket(lslidar_c16_msgs::LslidarC16Packet *pkt, const double time_offset) = 0;
int getRpm(void);
int getReturnMode(void);
bool getUpdateFlag(void);
void clearUpdateFlag(void);
protected:
ros::NodeHandle private_nh_;
uint16_t port_;
std::string devip_str_;
int cur_rpm_;
int return_mode_;
bool npkt_update_flag_;
bool add_multicast;
std::string group_ip;
};
/** @brief Live lslidar input from socket. */
class InputSocket : public Input {
public:
InputSocket(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER);
virtual ~InputSocket();
virtual int getPacket(lslidar_c16_msgs::LslidarC16Packet *pkt, const double time_offset);
private:
private:
int sockfd_;
in_addr devip_;
};
/** @brief lslidar input from PCAP dump file.
*
* Dump files can be grabbed by libpcap
*/
class InputPCAP : public Input {
public:
InputPCAP(ros::NodeHandle private_nh, uint16_t port = MSOP_DATA_PORT_NUMBER, double packet_rate = 0.0,
std::string filename = "", bool read_once = false, bool read_fast = false, double repeat_delay = 0.0);
virtual ~InputPCAP();
virtual int getPacket(lslidar_c16_msgs::LslidarC16Packet *pkt, const double time_offset);
private:
ros::Rate packet_rate_;
std::string filename_;
pcap_t *pcap_;
bpf_program pcap_packet_filter_;
char errbuf_[PCAP_ERRBUF_SIZE];
bool empty_;
bool read_once_;
bool read_fast_;
double repeat_delay_;
};
}
#endif // __LSLIDAR_INPUT_H
sensors/lslidar_C16/lslidar_c16_driver/include/lslidar_c16_driver/lslidar_c16_driver.h
+99
View File
@@ -0,0 +1,99 @@
/*
* This file is part of lslidar_c16 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef _LS_C16_DRIVER_H_
#define _LS_C16_DRIVER_H_
#include <string>
#include <ros/ros.h>
#include <ros/package.h>
#include <std_msgs/Int32.h>
#include <pcl/point_types.h>
#include <pcl_ros/impl/transforms.hpp>
#include <pcl_conversions/pcl_conversions.h>
#include "input.h"
#include <lslidar_c16_msgs/LslidarC16ScanUnified.h>
#include <lslidar_c16_msgs/lslidar_c16_control.h>
namespace lslidar_c16_driver
{
class lslidarDriver
{
public:
/**
* @brief lslidarDriver
* @param node raw packet output topic
* @param private_nh 通过这个节点传参数
*/
lslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh);
~lslidarDriver();
bool poll(void);
void difopPoll(void);
bool lslidarC16Control(lslidar_c16_msgs::lslidar_c16_control::Request &req,lslidar_c16_msgs::lslidar_c16_control::Response &res);
bool bSwitch = true;
private:
/// Callback for skip num for time synchronization
void skipNumCallback(const std_msgs::Int32::ConstPtr& skip_num);
bool SendPacketTolidar(bool power_switch);
// configuration parameters
struct
{
std::string frame_id; ///< tf frame ID
std::string model; ///< device model name
int npackets; ///< number of packets to collect
double rpm; ///< device rotation rate (RPMs)
double time_offset; ///< time in seconds added to each time stamp
int cut_angle;
int return_mode; //return wave number
} config_;
boost::shared_ptr<Input> msop_input_;
boost::shared_ptr<Input> difop_input_;
ros::Publisher msop_output_;
ros::Publisher difop_output_;
ros::Publisher output_sync_;
ros::ServiceServer lslidar_control;
int switch_status;
unsigned char difop_data[1206];
// Converter convtor_
boost::shared_ptr<boost::thread> difop_thread_;
// add for time synchronization
bool time_synchronization_;
unsigned char packetTimeStamp[10];
uint64_t pointcloudTimeStamp;
uint64_t GPSStableTS;
uint64_t GPSCountingTS;
uint64_t last_FPGA_ts;
uint64_t GPS_ts;
int cnt_gps_ts;
ros::Time timeStamp;
uint64_t usec_start;
std::string device_ip_string;
bool scan_fill;
lslidar_c16_msgs::LslidarC16ScanUnified scan_start;
};
} // namespace lslidar_driver
#endif
sensors/lslidar_C16/lslidar_c16_driver/launch/lslidar_c16_driver_nodelet.launch
+20
View File
@@ -0,0 +1,20 @@
<launch>
<arg name="device_ip" value="192.168.1.200"/>
<arg name="firing_port" value="2368"/>
<arg name="topic_packet" default="/apollo/sensor/velodyne16/LslidarPacket"/>
<arg name="topic_pointcloud" default="/apollo/sensor/velodyne16/PointCloud2"/>
<!--arg name="manager_name" default="nodelet_manager"/-->
<!-- start nodelet manager and load driver nodelet -->
<node pkg="nodelet" type="nodelet" name="lslidar_c16_nodelet_manager" args="manager" output="screen" respawn="true"/>
<node pkg="nodelet" type="nodelet"
name="lslidar_c16_driver_nodelet"
args="load lslidar_c16_driver/DriverNodelet lslidar_c16_nodelet_manager" respawn="true">
<param name="frame_id" value="laser_link"/>
</node>
</launch>
sensors/lslidar_C16/lslidar_c16_driver/launch/lslidar_c16_node.launch
+7
View File
@@ -0,0 +1,7 @@
<launch>
<node pkg="lslidar_c16_driver" type="lslidar_c16_driver_node" name="lslidar_c16_driver" >
<param name="frame_id" value="lslidar"/>
<param name="device_ip" value="192.168.1.200"/>
</node>
</launch>
sensors/lslidar_C16/lslidar_c16_driver/nodelet_lslidar_c16.xml
+9
View File
@@ -0,0 +1,9 @@
<library path="lib/liblslidar_c16_driver_nodelet">
<class name="lslidar_c16_driver/DriverNodelet"
type="lslidar_c16_driver::DriverNodelet"
base_class_type="nodelet::Nodelet">
<description>
Publish one lslidar raw data packet each time.
</description>
</class>
</library>
sensors/lslidar_C16/lslidar_c16_driver/package.xml
+58
View File
@@ -0,0 +1,58 @@
<?xml version="1.0"?>
<package>
<name>lslidar_c16_driver</name>
<version>1.0.0</version>
<maintainer email="tongsky723@sina.com">tongsky</maintainer>
<author>Yutong</author>
<license>BSD</license>
<description>
ROS device driver for Leishen C16 lidar.
</description>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>angles</build_depend>
<build_depend>nodelet</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>pluginlib</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>dynamic_reconfigure</build_depend>
<build_depend>roslaunch</build_depend>
<build_depend>rostest</build_depend>
<build_depend>tf2_ros</build_depend>
<build_depend>message_generation</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>rslidar_input</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_depend>pcl_ros</build_depend>
<build_depend>libpcl-all-dev</build_depend>
<build_depend>lslidar_c16_msgs</build_depend>
<run_depend>angles</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>nodelet</run_depend>
<run_depend>pluginlib</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>rospy</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>tf</run_depend>
<run_depend>dynamic_reconfigure</run_depend>
<run_depend>message_runtime</run_depend>
<run_depend>pcl_conversions</run_depend>
<run_depend>pcl_ros</run_depend>
<run_depend>libpcl-all</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>lslidar_c16_msgs</run_depend>
<export>
<nodelet plugin="${prefix}/nodelet_lslidar_c16.xml"/>
</export>
</package>
sensors/lslidar_C16/lslidar_c16_driver/src/input.cc
+386
View File
@@ -0,0 +1,386 @@
/*
* This file is part of lslidar_c16 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include "lslidar_c16_driver/input.h"
extern volatile sig_atomic_t flag;
namespace lslidar_c16_driver
{
static const size_t packet_size = sizeof(lslidar_c16_msgs::LslidarC16Packet().data);
////////////////////////////////////////////////////////////////////////
// Input base class implementation
////////////////////////////////////////////////////////////////////////
/** @brief constructor
*
* @param private_nh ROS private handle for calling node.
* @param port UDP port number.
*/
Input::Input(ros::NodeHandle private_nh, uint16_t port) : private_nh_(private_nh), port_(port)
{
npkt_update_flag_ = false;
cur_rpm_ = 0;
return_mode_ = 1;
private_nh.param("device_ip", devip_str_, std::string(""));
private_nh.param<bool>("add_multicast", add_multicast, false);
private_nh.param<std::string>("group_ip", group_ip, "224.1.1.2");
if (!devip_str_.empty())
ROS_INFO_STREAM("Only accepting packets from IP address: " << devip_str_);
}
int Input::getRpm(void)
{
return cur_rpm_;
}
int Input::getReturnMode(void)
{
return return_mode_;
}
bool Input::getUpdateFlag(void)
{
return npkt_update_flag_;
}
void Input::clearUpdateFlag(void)
{
npkt_update_flag_ = false;
}
////////////////////////////////////////////////////////////////////////
// InputSocket class implementation
////////////////////////////////////////////////////////////////////////
/** @brief constructor
*
* @param private_nh ROS private handle for calling node.
* @param port UDP port number
*/
InputSocket::InputSocket(ros::NodeHandle private_nh, uint16_t port) : Input(private_nh, port)
{
sockfd_ = -1;
if (!devip_str_.empty())
{
inet_aton(devip_str_.c_str(), &devip_);
}
ROS_INFO_STREAM("Opening UDP socket: port " << port);
sockfd_ = socket(PF_INET, SOCK_DGRAM, 0);
if (sockfd_ == -1)
{
perror("socket"); // TODO: ROS_ERROR errno
return;
}
int opt = 1;
if (setsockopt(sockfd_, SOL_SOCKET, SO_REUSEADDR, (const void*)&opt, sizeof(opt)))
{
perror("setsockopt error!\n");
return;
}
sockaddr_in my_addr; // my address information
memset(&my_addr, 0, sizeof(my_addr)); // initialize to zeros
my_addr.sin_family = AF_INET; // host byte order
my_addr.sin_port = htons(port); // port in network byte order
my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
if (bind(sockfd_, (sockaddr*)&my_addr, sizeof(sockaddr)) == -1)
{
perror("bind"); // TODO: ROS_ERROR errno
return;
}
if (add_multicast) {
struct ip_mreq group;
group.imr_multiaddr.s_addr = inet_addr(group_ip.c_str());
group.imr_interface.s_addr = htonl(INADDR_ANY);
//group.imr_interface.s_addr = inet_addr("192.168.1.102");
if (setsockopt(sockfd_, IPPROTO_IP, IP_ADD_MEMBERSHIP, (char *) &group, sizeof(group)) < 0) {
perror("Adding multicast group error ");
close(sockfd_);
exit(1);
} else
printf("Adding multicast group...OK.\n");
}
if (fcntl(sockfd_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
perror("non-block");
return;
}
}
/** @brief destructor */
InputSocket::~InputSocket(void)
{
(void)close(sockfd_);
}
/** @brief Get one lslidar packet. */
int InputSocket::getPacket(lslidar_c16_msgs::LslidarC16Packet* pkt, const double time_offset)
{
double time1 = ros::Time::now().toSec();
struct pollfd fds[1];
fds[0].fd = sockfd_;
fds[0].events = POLLIN;
static const int POLL_TIMEOUT = 3000; // one second (in msec)
sockaddr_in sender_address;
socklen_t sender_address_len = sizeof(sender_address);
while (flag == 1)
// while (true)
{
// Receive packets that should now be available from the
// socket using a blocking read.
// poll() until input available
do
{
int retval = poll(fds, 1, POLL_TIMEOUT);
if (retval < 0) // poll() error?
{
if (errno != EINTR)
ROS_ERROR("poll() error: %s", strerror(errno));
return 1;
}
if (retval == 0) // poll() timeout?
{
time_t curTime = time(NULL);
struct tm *curTm = localtime(&curTime);
char bufTime[30] = {0};
sprintf(bufTime,"%d-%d-%d %d:%d:%d", curTm->tm_year+1900, curTm->tm_mon+1,
curTm->tm_mday, curTm->tm_hour, curTm->tm_min, curTm->tm_sec);
ROS_WARN_THROTTLE(2, "%s lslidar poll() timeout", bufTime);
/*
char buffer_data[8] = "re-con";
memset(&sender_address, 0, sender_address_len); // initialize to zeros
sender_address.sin_family = AF_INET; // host byte order
sender_address.sin_port = htons(MSOP_DATA_PORT_NUMBER); // port in network byte order, set any value
sender_address.sin_addr.s_addr = devip_.s_addr; // automatically fill in my IP
sendto(sockfd_, &buffer_data, strlen(buffer_data), 0, (sockaddr*)&sender_address, sender_address_len);
*/
return 1;
}
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL)) // device error?
{
ROS_ERROR("poll() reports lslidar error");
return 1;
}
} while ((fds[0].revents & POLLIN) == 0);
ssize_t nbytes = recvfrom(sockfd_, &pkt->data[0], packet_size, 0, (sockaddr*)&sender_address, &sender_address_len);
if (nbytes < 0)
{
if (errno != EWOULDBLOCK)
{
perror("recvfail");
ROS_INFO("recvfail");
return 1;
}
}
else if ((size_t)nbytes == packet_size)
{
if (devip_str_ != "" && sender_address.sin_addr.s_addr != devip_.s_addr)
continue;
else
break; // done
}
ROS_DEBUG_STREAM("incomplete lslidar packet read: " << nbytes << " bytes");
}
if (flag == 0)
{
abort();
}
if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 && pkt->data[3] == 0x5A)
{//difop
int rpm = (pkt->data[8]<<8)|pkt->data[9];
if (rpm < 200){
rpm = 600;
}
//ROS_INFO("rpm=%d",rpm);
int mode = 1;
if (cur_rpm_ != rpm || return_mode_ != mode)
{
cur_rpm_ = rpm;
/* if((pkt->data[8] == 0x00) && (pkt->data[9] ==0x2f)){
cur_rpm_ = 600;
}*/
return_mode_ = mode;
npkt_update_flag_ = true;
}
}
// Average the times at which we begin and end reading. Use that to
// estimate when the scan occurred. Add the time offset.
double time2 = ros::Time::now().toSec();
pkt->stamp = ros::Time((time2 + time1) / 2.0 + time_offset);
return 0;
}
////////////////////////////////////////////////////////////////////////
// InputPCAP class implementation
////////////////////////////////////////////////////////////////////////
/** @brief constructor
*
* @param private_nh ROS private handle for calling node.
* @param port UDP port number
* @param packet_rate expected device packet frequency (Hz)
* @param filename PCAP dump file name
*/
InputPCAP::InputPCAP(ros::NodeHandle private_nh, uint16_t port, double packet_rate, std::string filename,
bool read_once, bool read_fast, double repeat_delay)
: Input(private_nh, port), packet_rate_(packet_rate), filename_(filename)
{
pcap_ = NULL;
empty_ = true;
// get parameters using private node handle
private_nh.param("read_once", read_once_, false);
private_nh.param("read_fast", read_fast_, false);
private_nh.param("repeat_delay", repeat_delay_, 0.0);
if (read_once_)
ROS_INFO("Read input file only once.");
if (read_fast_)
ROS_INFO("Read input file as quickly as possible.");
if (repeat_delay_ > 0.0)
ROS_INFO("Delay %.3f seconds before repeating input file.", repeat_delay_);
// Open the PCAP dump file
// ROS_INFO("Opening PCAP file \"%s\"", filename_.c_str());
ROS_INFO_STREAM("Opening PCAP file " << filename_);
if ((pcap_ = pcap_open_offline(filename_.c_str(), errbuf_)) == NULL)
{
ROS_FATAL("Error opening lslidar socket dump file.");
return;
}
std::stringstream filter;
if (devip_str_ != "") // using specific IP?
{
filter << "src host " << devip_str_ << " && ";
}
filter << "udp dst port " << port;
pcap_compile(pcap_, &pcap_packet_filter_, filter.str().c_str(), 1, PCAP_NETMASK_UNKNOWN);
}
/** destructor */
InputPCAP::~InputPCAP(void)
{
pcap_close(pcap_);
}
/** @brief Get one lslidar packet. */
int InputPCAP::getPacket(lslidar_c16_msgs::LslidarC16Packet* pkt, const double time_offset)
{
struct pcap_pkthdr* header;
const u_char* pkt_data;
// while (flag == 1)
while (true)
{
int res;
if ((res = pcap_next_ex(pcap_, &header, &pkt_data)) >= 0)
{
// Skip packets not for the correct port and from the
// selected IP address.
if (!devip_str_.empty() && (0 == pcap_offline_filter(&pcap_packet_filter_, header, pkt_data)))
continue;
// Keep the reader from blowing through the file.
if (read_fast_ == false)
packet_rate_.sleep();
memcpy(&pkt->data[0], pkt_data + 42, packet_size);
if (pkt->data[0] == 0xA5 && pkt->data[1] == 0xFF && pkt->data[2] == 0x00 && pkt->data[3] == 0x5A)
{//difop
int rpm = (pkt->data[8]<<8)|pkt->data[9];
int mode = 1;
if ((pkt->data[45] == 0x08 && pkt->data[46] == 0x02 && pkt->data[47] >= 0x09) || (pkt->data[45] > 0x08)
|| (pkt->data[45] == 0x08 && pkt->data[46] > 0x02))
{
if (pkt->data[300] != 0x01 && pkt->data[300] != 0x02)
{
mode = 0;
}
}
if (cur_rpm_ != rpm || return_mode_ != mode)
{
cur_rpm_ = rpm;
return_mode_ = mode;
npkt_update_flag_ = true;
}
}
pkt->stamp = ros::Time::now(); // time_offset not considered here, as no
// synchronization required
empty_ = false;
return 0; // success
}
if (empty_) // no data in file?
{
ROS_WARN("Error %d reading lslidar packet: %s", res, pcap_geterr(pcap_));
return -1;
}
if (read_once_)
{
ROS_INFO("end of file reached -- done reading.");
return -1;
}
if (repeat_delay_ > 0.0)
{
ROS_INFO("end of file reached -- delaying %.3f seconds.", repeat_delay_);
usleep(rint(repeat_delay_ * 1000000.0));
}
ROS_DEBUG("replaying lslidar dump file");
// I can't figure out how to rewind the file, because it
// starts with some kind of header. So, close the file
// and reopen it with pcap.
pcap_close(pcap_);
pcap_ = pcap_open_offline(filename_.c_str(), errbuf_);
empty_ = true; // maybe the file disappeared?
} // loop back and try again
if (flag == 0)
{
abort();
}
}
}
sensors/lslidar_C16/lslidar_c16_driver/src/lslidar_c16_driver.cpp
+371
View File
@@ -0,0 +1,371 @@
/*
* Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
* Copyright (C) 2009-2012 Austin Robot Technology, Jack O'Quin
* Copyright (C) 2017 Robosense, Tony Zhang
*
* License: Modified BSD Software License Agreement
*
* $Id$
*/
/** \file
*
* ROS driver implementation for the RILIDAR 3D LIDARs
*/
#include "lslidar_c16_driver/lslidar_c16_driver.h"
#include <lslidar_c16_msgs/LslidarC16ScanUnified.h>
#include <lslidar_c16_driver/lslidar_c16_driver.h>
#include <std_msgs/String.h>
namespace lslidar_c16_driver {
static const unsigned int POINTS_ONE_CHANNEL_PER_SECOND = 20000;
static const unsigned int BLOCKS_ONE_CHANNEL_PER_PKT = 12;
lslidarDriver::lslidarDriver(ros::NodeHandle node, ros::NodeHandle private_nh) : switch_status(0) {
scan_fill = false;
// use private node handle to get parameters
private_nh.param("frame_id", config_.frame_id, std::string("lslidar"));
private_nh.param("device_ip", device_ip_string, std::string("192.168.1.200"));
std::string tf_prefix = tf::getPrefixParam(private_nh);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
config_.frame_id = tf::resolve(tf_prefix, config_.frame_id);
// get model name, validate string, determine packet rate
private_nh.param("model", config_.model, std::string("LSC16"));
double packet_rate; // packet frequency (Hz)
packet_rate = 840; //20000/24
private_nh.param("rpm", config_.rpm, 300.0);
private_nh.param("return_mode", config_.return_mode, 1);
double frequency = (config_.rpm / 60.0); // expected Hz rate
printf("driver return mode = %d\n", config_.return_mode);
// default number of packets for each scan is a single revolution
// (fractions rounded up)
int npackets = (int) ceil(packet_rate / frequency);
private_nh.param("npackets", config_.npackets, npackets);
ROS_INFO_STREAM("publishing " << config_.npackets << " packets per scan");
std::string dump_file;
private_nh.param("pcap", dump_file, std::string(""));
int msop_udp_port;
private_nh.param("msop_port", msop_udp_port, (int) MSOP_DATA_PORT_NUMBER);
int difop_udp_port;
private_nh.param("difop_port", difop_udp_port, (int) DIFOP_DATA_PORT_NUMBER);
scan_start = lslidar_c16_msgs::LslidarC16ScanUnified();
scan_start.packets.resize(1);
// open rslidar input device or file
if (dump_file != "") // have PCAP file?
{
// read data from packet capture file
msop_input_.reset(new lslidar_c16_driver::InputPCAP(private_nh, msop_udp_port, packet_rate, dump_file));
difop_input_.reset(new lslidar_c16_driver::InputPCAP(private_nh, difop_udp_port, packet_rate, dump_file));
} else {
// read data from live socket
msop_input_.reset(new lslidar_c16_driver::InputSocket(private_nh, msop_udp_port));
difop_input_.reset(new lslidar_c16_driver::InputSocket(private_nh, difop_udp_port));
}
// raw packet output topic
std::string output_packets_topic;
private_nh.param("output_packets_topic", output_packets_topic, std::string("lslidar_packet_c16"));
msop_output_ = node.advertise<lslidar_c16_msgs::LslidarC16ScanUnified>(output_packets_topic, 10);
std::string output_difop_topic;
private_nh.param("output_difop_topic", output_difop_topic, std::string("lslidar_packet_difop_c16"));
difop_output_ = node.advertise<lslidar_c16_msgs::LslidarC16Packet>(output_difop_topic, 10);
lslidar_control = node.advertiseService("lslidarcontrol", &lslidarDriver::lslidarC16Control, this);
difop_thread_ = boost::shared_ptr<boost::thread>(
new boost::thread(boost::bind(&lslidarDriver::difopPoll, this)));
private_nh.param("time_synchronization", time_synchronization_, false);
if (time_synchronization_) {
output_sync_ = node.advertise<sensor_msgs::TimeReference>("sync_header", 1);
}
memset(difop_data, 0, 1206);
}
lslidarDriver::~lslidarDriver() {
if (difop_thread_ != NULL) {
printf("error");
difop_thread_->interrupt();
difop_thread_->join();
}
}
/** poll the device
*
* @returns true unless end of file reached
*/
bool lslidarDriver::poll(void) { // Allocate a new shared pointer for zero-copy sharing with other nodelets.
lslidar_c16_msgs::LslidarC16ScanUnifiedPtr scan(new lslidar_c16_msgs::LslidarC16ScanUnified);
// Since the rslidar delivers data at a very high rate, keep
// reading and publishing scans as fast as possible.
int mode = config_.return_mode;
uint64_t GPSCurrentTS;
if (difop_input_->getUpdateFlag()) {
int packets_rate = ceil(POINTS_ONE_CHANNEL_PER_SECOND / BLOCKS_ONE_CHANNEL_PER_PKT);
packets_rate = ceil(packets_rate / 2);
config_.rpm = difop_input_->getRpm();
// if(config_.rpm >= 300 && config_.rpm < 600){
// config_.rpm = 300;
// }else if(config_.rpm >= 600 && config_.rpm < 1200){
// config_.rpm = 600;
// }else if(config_.rpm >= 1200){
// config_.rpm = 1200;
// }
//config_.npackets = ceil(packets_rate * 60 / config_.rpm) * mode;
config_.npackets = ceil(packets_rate * 60 / config_.rpm) * mode;
config_.npackets = config_.npackets * 11 / 10;
difop_input_->clearUpdateFlag();
//ROS_INFO("packet rate is %d, rpm is %3.3f, npacket is %d", packets_rate, config_.rpm, config_.npackets);
}
//ROS_INFO("rpm is %3.3f, npacket is %d", config_.rpm, config_.npackets);
scan->packets.clear();
scan->packets.resize(config_.npackets);
int azi1, azi2;
if (scan_fill) {
scan->packets[0] = scan_start.packets[0];
GPSCurrentTS = GPSCountingTS;
} else {
while (true) {
while (true) {
int rc = msop_input_->getPacket(&scan->packets[0], config_.time_offset);
if (rc == 0)
break;
if (rc < 0)
return false;
}
azi1 = 256 * scan->packets[0].data[3] + scan->packets[0].data[2];
azi2 = 256 * scan->packets[0].data[1103] + scan->packets[0].data[1102];
if (azi1 > 35000 && azi2 < 1000) break;
}
}
scan_fill = false;
// use in standard behaviour only
for (int i = 1; i < config_.npackets; ++i) {
while (true) {
// keep reading until full packet received
//ROS_INFO_STREAM("time_offset: " << config_.time_offset);
int rc = msop_input_->getPacket(&scan->packets[i], config_.time_offset);
if (rc == 0)
break; // got a full packet?
if (rc < 0)
return false; // end of file reached?
}
azi1 = 256 * scan->packets[i].data[3] + scan->packets[i].data[2];
azi2 = 256 * scan->packets[i].data[1103] + scan->packets[i].data[1102];
//azi2 = (azi2 +20) % 36000;
//if ( (azi1 > 35800 && azi2 < 100 )) {
if ((azi1 > 35000 && azi2 < 1000) || (azi1 < 500 && i > config_.npackets / 2)) {
scan_fill = true;
scan_start.packets[0] = scan->packets[i];
// ROS_INFO_STREAM("azi1: " << azi1 <<" "<< "azi2: " << azi2 << " i:" << i);
break;
}
}
if (time_synchronization_) {
sensor_msgs::TimeReference sync_header;
// it is already the msop msg
// use the first packets
lslidar_c16_msgs::LslidarC16Packet pkt = scan->packets[0];
uint64_t packet_timestamp;
static uint64_t last_gps_time; //上一个设备包的gps时间
static uint64_t last_packet_seconds; //上一个数据包的时间戳
packet_timestamp = (pkt.data[1200] +
pkt.data[1201] * pow(2, 8) +
pkt.data[1202] * pow(2, 16) +
pkt.data[1203] * pow(2, 24)) * 1e3; //ns
//timeStamp = ros::Time(GPSCurrentTS, packet_timestamp);// s,ns
if (last_packet_seconds > 800000000 && packet_timestamp < 200000000) {
timeStamp = ros::Time(GPSCurrentTS, packet_timestamp);
} else {
timeStamp = ros::Time(last_gps_time, packet_timestamp);
}
last_gps_time = GPSCurrentTS;
last_packet_seconds = packet_timestamp;
//ROS_INFO("Lidar_time: %f, GPS_time:%lu, fpga_time: ns:%lu",timeStamp.toSec(), GPSCurrentTS, packet_timestamp);
sync_header.header.stamp = timeStamp;
output_sync_.publish(sync_header);
}
// publish message using time of last packet read
// ROS_INFO("Publishing a full scan.");
if (time_synchronization_) {
scan->header.stamp = timeStamp;
} else {
//scan->header.stamp = scan->packets.back().stamp;
scan->header.stamp = ros::Time::now();
}
scan->header.frame_id = config_.frame_id;
msop_output_.publish(scan);
return true;
}
void lslidarDriver::difopPoll(void) {
// reading and publishing scans as fast as possible.
lslidar_c16_msgs::LslidarC16PacketPtr difop_packet_ptr(new lslidar_c16_msgs::LslidarC16Packet);
while (ros::ok()) {
// keep reading
lslidar_c16_msgs::LslidarC16Packet difop_packet_msg;
int rc = difop_input_->getPacket(&difop_packet_msg, config_.time_offset);
if (rc == 0) {
for (int i = 0; i < 1206; i++) {
difop_data[i] = difop_packet_msg.data[i];
}
//std::cout << "Publishing a difop data." << std::endl;
ROS_DEBUG("Publishing a difop data.");
*difop_packet_ptr = difop_packet_msg;
difop_output_.publish(difop_packet_ptr);
int version_data = difop_packet_msg.data[1202];
if (2 == version_data) {
this->packetTimeStamp[4] = difop_packet_msg.data[41];
this->packetTimeStamp[5] = difop_packet_msg.data[40];
this->packetTimeStamp[6] = difop_packet_msg.data[39];
this->packetTimeStamp[7] = difop_packet_msg.data[38];
this->packetTimeStamp[8] = difop_packet_msg.data[37];
this->packetTimeStamp[9] = difop_packet_msg.data[36];
} else {
this->packetTimeStamp[4] = difop_packet_msg.data[57];
this->packetTimeStamp[5] = difop_packet_msg.data[56];
this->packetTimeStamp[6] = difop_packet_msg.data[55];
this->packetTimeStamp[7] = difop_packet_msg.data[54];
this->packetTimeStamp[8] = difop_packet_msg.data[53];
this->packetTimeStamp[9] = difop_packet_msg.data[52];
}
//ROS_INFO_STREAM("time: " << difop_packet_msg.data[57]);
//ROS_INFO_STREAM("time: " << difop_packet_msg.data[56]);
struct tm cur_time;
memset(&cur_time, 0, sizeof(cur_time));
cur_time.tm_sec = this->packetTimeStamp[4] + 1;
cur_time.tm_min = this->packetTimeStamp[5];
cur_time.tm_hour = this->packetTimeStamp[6] + 8;
cur_time.tm_mday = this->packetTimeStamp[7];
cur_time.tm_mon = this->packetTimeStamp[8] - 1;
cur_time.tm_year = this->packetTimeStamp[9] + 2000 - 1900;
this->pointcloudTimeStamp = mktime(&cur_time);
if (GPSCountingTS != this->pointcloudTimeStamp) {
cnt_gps_ts = 0;
GPSCountingTS = this->pointcloudTimeStamp;
// ROS_ERROR("GPSCountingTS=%lu",GPSCountingTS);
//to beijing time printing
//ROS_INFO("GPS: y:%d m:%d d:%d h:%d m:%d s:%d",cur_time.tm_year+1900,cur_time.tm_mon+1,cur_time.tm_mday,cur_time.tm_hour+8,cur_time.tm_min,cur_time.tm_sec);
} else if (cnt_gps_ts == 3) {
GPSStableTS = GPSCountingTS;
} else {
cnt_gps_ts++;
}
}
if (rc < 0)
return; // end of file reached?
ros::spinOnce();
}
}
bool lslidarDriver::lslidarC16Control(lslidar_c16_msgs::lslidar_c16_control::Request &req,
lslidar_c16_msgs::lslidar_c16_control::Response &res) {
ROS_WARN("--------------------------");
// sleep(1);
lslidar_c16_msgs::LslidarC16Packet packet0;
packet0.data[0] = 0x00;
packet0.data[1] = 0x00;
int rc_msop = -1;
if (req.LaserControl == 1) {
if ((rc_msop = msop_input_->getPacket(&packet0, config_.time_offset)) == 0) {
res.status = "already power on status";
return true;
}
ROS_WARN("receive cmd: %d,power on", req.LaserControl);
SendPacketTolidar(true);
double time1 = ros::Time::now().toSec();
do {
rc_msop = msop_input_->getPacket(&packet0, config_.time_offset);
double time2 = ros::Time::now().toSec();
if(time2 - time1 > 20 ){
res.status = "lidar connect error";
return true;
}
} while ((rc_msop != 0) && (packet0.data[0] != 0xff) && (packet0.data[1] != 0xee));
res.status = "pow on";
} else if (req.LaserControl == 0) {
ROS_WARN("receive cmd: %d,power off", req.LaserControl);
SendPacketTolidar(false);
res.status = "power off";
} else {
res.status = "cmd error";
}
return true;
}
bool lslidarDriver::SendPacketTolidar(bool power_switch) {
int socketid;
unsigned char config_data[1206];
//int data_port = difop_data[24] * 256 + difop_data[25];
mempcpy(config_data, difop_data, 1206);
config_data[0] = 0xAA;
config_data[1] = 0x00;
config_data[2] = 0xFF;
config_data[3] = 0x11;
config_data[4] = 0x22;
config_data[5] = 0x22;
config_data[6] = 0xAA;
config_data[7] = 0xAA;
if (power_switch) {
config_data[45] = 0x00;
} else {
config_data[45] = 0x01;
}
if (config_data[8] == 0x00) {
config_data[8] = 0x02;
config_data[9] = 0x58;
}
sockaddr_in addrSrv;
socketid = socket(2, 2, 0);
addrSrv.sin_addr.s_addr = inet_addr(device_ip_string.c_str());
addrSrv.sin_family = AF_INET;
addrSrv.sin_port = htons(2368);
sendto(socketid, (const char *) config_data, 1206, 0, (struct sockaddr *) &addrSrv, sizeof(addrSrv));
return 0;
}
// add for time synchronization
} // namespace lslidar_c16_driver
sensors/lslidar_C16/lslidar_c16_driver/src/lslidar_c16_driver_node.cpp
+52
View File
@@ -0,0 +1,52 @@
/*
* This file is part of lslidar_c16 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include <ros/ros.h>
#include "lslidar_c16_driver/lslidar_c16_driver.h"
#include "std_msgs/String.h"
using namespace lslidar_c16_driver;
volatile sig_atomic_t flag = 1;
static void my_handler(int sig)
{
flag = 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "lslidar_c16_driver");
ros::NodeHandle node;
ros::NodeHandle private_nh("~");
signal(SIGINT, my_handler);
// start the driver
lslidar_c16_driver::lslidarDriver dvr(node, private_nh);
// loop until shut down or end of file
dvr.bSwitch = true;
while (ros::ok())
{
if(dvr.bSwitch){
dvr.poll();
}
ros::spinOnce();
}
return 0;
}
sensors/lslidar_C16/lslidar_c16_driver/src/lslidar_c16_driver_nodelet.cc
+82
View File
@@ -0,0 +1,82 @@
/*
* This file is part of lslidar_c16 driver.
*
* The driver is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* The driver is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with the driver. If not, see <http://www.gnu.org/licenses/>.
*/
#include <string>
#include <boost/thread.hpp>
#include <ros/ros.h>
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include "lslidar_c16_driver/lslidar_c16_driver.h"
volatile sig_atomic_t flag = 1;
namespace lslidar_c16_driver
{
class DriverNodelet : public nodelet::Nodelet
{
public:
DriverNodelet() : running_(false)
{
}
~DriverNodelet()
{
if (running_)
{
NODELET_INFO("shutting down driver thread");
running_ = false;
deviceThread_->join();
NODELET_INFO("driver thread stopped");
}
}
private:
virtual void onInit(void);
virtual void devicePoll(void);
volatile bool running_; ///< device thread is running
boost::shared_ptr<boost::thread> deviceThread_;
boost::shared_ptr<lslidarDriver> dvr_; ///< driver implementation class
};
void DriverNodelet::onInit()
{
// start the driver
dvr_.reset(new lslidarDriver(getNodeHandle(), getPrivateNodeHandle()));
// spawn device poll thread
running_ = true;
deviceThread_ = boost::shared_ptr<boost::thread>(new boost::thread(boost::bind(&DriverNodelet::devicePoll, this)));
// NODELET_INFO("DriverNodelet onInit");
}
/** @brief Device poll thread main loop. */
void DriverNodelet::devicePoll()
{
while (ros::ok() && dvr_->poll())
{
ros::spinOnce();
}
}
}
//
// parameters are: class type, base class type
PLUGINLIB_EXPORT_CLASS(lslidar_c16_driver::DriverNodelet, nodelet::Nodelet)
sensors/lslidar_C16/lslidar_c16_msgs/CMakeLists.txt
+32
View File
@@ -0,0 +1,32 @@
cmake_minimum_required(VERSION 2.8.3)
project(lslidar_c16_msgs)
find_package(catkin REQUIRED COMPONENTS
message_generation
std_msgs
sensor_msgs
)
add_message_files(
DIRECTORY msg
FILES
LslidarC16Layer.msg
LslidarC16Packet.msg
LslidarC16Point.msg
LslidarC16Scan.msg
LslidarC16Sweep.msg
LslidarC16ScanUnified.msg
)
add_service_files(
FILES
lslidar_c16_control.srv
)
generate_messages(DEPENDENCIES std_msgs sensor_msgs)
catkin_package(
CATKIN_DEPENDS message_runtime std_msgs sensor_msgs
)
sensors/lslidar_C16/lslidar_c16_msgs/msg/LslidarC16Layer.msg
+3
View File
@@ -0,0 +1,3 @@
# Header header
# The 0th scan is at the bottom
sensor_msgs/LaserScan[16] scan_channel
sensors/lslidar_C16/lslidar_c16_msgs/msg/LslidarC16Packet.msg
+5
View File
@@ -0,0 +1,5 @@
# Raw Leishen LIDAR packet.
time stamp # packet timestamp
uint8[1206] data # packet contents
sensors/lslidar_C16/lslidar_c16_msgs/msg/LslidarC16Point.msg
+12
View File
@@ -0,0 +1,12 @@
# Time when the point is captured
float64 time
# Converted distance in the sensor frame
float64 x
float64 y
float64 z
# Raw measurement from Leishen C16
float64 azimuth
float64 distance
float64 intensity
sensors/lslidar_C16/lslidar_c16_msgs/msg/LslidarC16Scan.msg
+6
View File
@@ -0,0 +1,6 @@
# Altitude of all the points within this scan
float64 altitude
# The valid points in this scan sorted by azimuth
# from 0 to 359.99
LslidarC16Point[] points
sensors/lslidar_C16/lslidar_c16_msgs/msg/LslidarC16ScanUnified.msg
+5
View File
@@ -0,0 +1,5 @@
#lslidar C16 scan packets
Header header #standard ros message header
uint64 basetime #time to the top of hour
LslidarC16Packet[] packets #vector of raw packets
sensors/lslidar_C16/lslidar_c16_msgs/msg/LslidarC16Sweep.msg
+4
View File
@@ -0,0 +1,4 @@
Header header
# The 0th scan is at the bottom
LslidarC16Scan[16] scans
sensors/lslidar_C16/lslidar_c16_msgs/package.xml
+22
View File
@@ -0,0 +1,22 @@
<package>
<name>lslidar_c16_msgs</name>
<version>1.2.0</version>
<description>
ROS message definitions for Leishen C16 LIDARs.
</description>
<url>http://www.ros.org/wiki/lslidar_c16</url>
<maintainer email="yutongyan@lslidar.com">Yutong</maintainer>
<author>Yutong</author>
<license>GNU General Public License V3.0</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<run_depend>message_runtime</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>std_msgs</run_depend>
</package>
sensors/lslidar_C16/lslidar_c16_msgs/srv/lslidar_c16_control.srv
+3
View File
@@ -0,0 +1,3 @@
int32 LaserControl
---
string status