HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
Devices/Packages/olei_lidar_driver_ros/olei_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 2.8.3)
project(olei_msgs)
find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
add_message_files(
DIRECTORY msg
FILES
oleiPacket.msg
oleiScan.msg
)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(
CATKIN_DEPENDS message_runtime std_msgs
)

5
Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiPacket.msg Executable file
View File

@@ -0,0 +1,5 @@
# Raw olei LIDAR packet.
time stamp # packet timestamp
uint8[1240] data # packet contents

4
Devices/Packages/olei_lidar_driver_ros/olei_msgs/msg/oleiScan.msg Executable file
View File

@@ -0,0 +1,4 @@
# olei LIDAR scan packets.
Header header # standard ROS message header
oleiPacket[] packets # vector of raw packets

23
Devices/Packages/olei_lidar_driver_ros/olei_msgs/package.xml Executable file
View File

@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package format="2">
<name>olei_msgs</name>
<version>1.5.2</version>
<description>
ROS message definitions for olei 2D LIDARs.
</description>
<maintainer email="emanuele.montemurro@olei.com">Emanuele</maintainer>
<author>Emanuele</author>
<license>BSD</license>
<url type="website">TODO</url>
<url type="repository">TODO</url>
<url type="bugtracker">TODO</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<depend>std_msgs</depend>
<exec_depend>message_runtime</exec_depend>
</package>

54
Devices/Packages/olei_lidar_driver_ros/olelidar/CMakeLists.txt Executable file
View File

@@ -0,0 +1,54 @@
cmake_minimum_required(VERSION 3.5)
project(olelidar)
set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_FLAGS "-std=c++11 -g ${CMAKE_CXX_FLAGS}")
find_package(catkin REQUIRED
COMPONENTS
roscpp
diagnostic_updater
dynamic_reconfigure
sensor_msgs
olei_msgs)
generate_dynamic_reconfigure_options(cfg/oleiPuck.cfg)
catkin_package()
#add_executable(${PROJECT_NAME}_driver src/driver.cpp)
add_library(${PROJECT_NAME}_driver src/driver.cpp)
target_include_directories(${PROJECT_NAME}_driver PUBLIC ${catkin_INCLUDE_DIRS} src)
target_link_libraries(${PROJECT_NAME}_driver PUBLIC ${catkin_LIBRARIES})
add_executable(${PROJECT_NAME}_decoder src/decoder.cpp)
target_include_directories(${PROJECT_NAME}_decoder PUBLIC ${catkin_INCLUDE_DIRS} src)
target_link_libraries(${PROJECT_NAME}_decoder PUBLIC ${catkin_LIBRARIES} ${PROJECT_NAME}_driver)
add_dependencies(${PROJECT_NAME}_driver ${PROJECT_NAME}_gencfg)
add_dependencies(${PROJECT_NAME}_decoder ${PROJECT_NAME}_gencfg)
## Mark executables and/or libraries for installation
install(TARGETS ${PROJECT_NAME}_driver ${PROJECT_NAME}_decoder
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/include
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".git" EXCLUDE
)
install(FILES
cfg/oleiPuck.cfg
launch/decoder.launch
launch/driver.launch
launch/scan.launch
# launch/olelidar.launch
launch/debug.conf
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
)

39
Devices/Packages/olei_lidar_driver_ros/olelidar/cfg/oleiPuck.cfg Executable file
View File

@@ -0,0 +1,39 @@
#!/usr/bin/env python
PACKAGE = "olelidar"
from dynamic_reconfigure.parameter_generator_catkin import *
gen = ParameterGenerator()
# device
gen.add("device_ip", str_t, 0, "IP addr to receive packet from.", "192.168.2.122")
gen.add("device_port", int_t, 0, "UDP port to receive packet from.", 2368)
gen.add("local_ip", str_t, 0, "loacl IP addr.", "192.168.2.198")
gen.add("multiaddr", str_t, 0, "multiaddr.", "239.255.0.100")
gen.add("freq", double_t, 0, "rotate degree per second", 25.0)
gen.add("route", int_t, 0, "rotate degree per second", 2000)
gen.add("step", double_t, 0, "rotate degree per second", 0.225)
# laserscan
'''
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
'''
# gen.add(name, type, level, description, default, min, max)
gen.add("frame_id", str_t, 0, "olelidar")
gen.add("angle_min", double_t, 0, "angle_min",0,0,360)
gen.add("angle_max", double_t, 360, "angle_max",360,0,360)
gen.add("range_min", double_t, 0.1, "min range", 0.1, 0.1, 20)
gen.add("range_max", double_t, 0.1, "max range", 50.0, 0.1, 50)
exit(gen.generate(PACKAGE, PACKAGE, "oleiPuck"))

1
Devices/Packages/olei_lidar_driver_ros/olelidar/launch/debug.conf Executable file
View File

@@ -0,0 +1 @@
log4j.logger.ros.olelidar=DEBUG

22
Devices/Packages/olei_lidar_driver_ros/olelidar/launch/decoder.launch Executable file
View File

@@ -0,0 +1,22 @@
<launch>
<arg name="pkg" value="olelidar"/>
<arg name="frame_id" default="olelidar"/>
<arg name="r_max" default="50"/>
<arg name="ang_start" default="0"/>
<arg name="ang_end" default="360"/>
<arg name="inverted" default="false"/>
<arg name="poly" default="1"/>
<node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen">
<param name="frame_id" type="string" value="$(arg frame_id)"/>
<param name="r_max" type="int" value="$(arg r_max)"/>
<param name="ang_start" type="int" value="$(arg ang_start)"/>
<param name="ang_end" type="int" value="$(arg ang_end)"/>
<param name="inverted" type="bool" value="$(arg inverted)"/>
<param name="poly" type="int" value="$(arg poly)"/>
<remap from="~packet" to="packet"/>
<remap from="~scan" to="scan"/>
</node>
</launch>

15
Devices/Packages/olei_lidar_driver_ros/olelidar/launch/driver.launch Executable file
View File

@@ -0,0 +1,15 @@
<launch>
<arg name="pkg" value="olelidar"/>
<arg name="device_ip" default="192.168.1.100"/>
<arg name="device_port" default="2368"/>
<arg name="local_ip" default="192.168.2.198"/>
<arg name="multiaddr" default="239.255.0.100"/>
<node pkg="olelidar" type="$(arg pkg)_driver" name="$(arg pkg)_driver" output="screen">
<param name="device_ip" type="string" value="$(arg device_ip)"/>
<param name="device_port" type="int" value="$(arg device_port)"/>
<param name="local_ip" type="string" value="$(arg local_ip)"/>
<param name="multiaddr" type="string" value="$(arg multiaddr)"/>
<remap from="~packet" to="packet"/>
</node>
</launch>

57
Devices/Packages/olei_lidar_driver_ros/olelidar/launch/scan.launch Executable file
View File

@@ -0,0 +1,57 @@
<launch>
<arg name="pkg" value="olelidar"/>
<!-- driver -->
<arg name="driver" default="true"/>
<arg name="device_ip_1" default="192.168.1.124"/>
<arg name="device_ip_2" default="192.168.2.13"/>
<arg name="device_port_1" default="2368"/>
<arg name="device_port_2" default="2369"/>
<arg name="local_ip" default="192.168.1.121"/>
<arg name="multiaddr" default=""/>
<!-- decoder -->
<arg name="frame_id_1" default="front_scan"/>
<arg name="frame_id_2" default="rear_scan"/>
<arg name="r_max_1" default="10"/>
<arg name="r_max_2" default="25"/>
<arg name="ang_start" default="-180"/>
<arg name="ang_end" default="180"/>
<arg name="decoder" default="true"/>
<arg name="inverted" default="false"/>
<arg name="debug" default="false"/>
<env if="$(arg debug)" name="ROSCONSOLE_CONFIG_FILE" value="$(find olelidar)/launch/debug.conf"/>
<node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen" ns="front_scan">
<param name="frame_id" type="string" value="$(arg frame_id_1)"/>
<param name="r_max" type="int" value="$(arg r_max_1)"/>
<param name="ang_start" type="int" value="$(arg ang_start)"/>
<param name="ang_end" type="int" value="$(arg ang_end)"/>
<param name="inverted" type="bool" value="$(arg inverted)"/>
<remap from="~packet" to="packet"/>
<remap from="~scan" to="/front_scan"/>
<param name="device_ip" type="string" value="$(arg device_ip_1)"/>
<param name="device_port" type="int" value="$(arg device_port_1)"/>
<param name="local_ip" type="string" value="$(arg local_ip)"/>
<param name="multiaddr" type="string" value="$(arg multiaddr)"/>
</node>
<!-- <node pkg="$(arg pkg)" type="$(arg pkg)_decoder" name="$(arg pkg)_decoder" output="screen" ns="rear_scan">
<param name="frame_id" type="string" value="$(arg frame_id_2)"/>
<param name="r_max" type="int" value="$(arg r_max_2)"/>
<param name="ang_start" type="int" value="$(arg ang_start)"/>
<param name="ang_end" type="int" value="$(arg ang_end)"/>
<param name="inverted" type="bool" value="$(arg inverted)"/>
<remap from="~packet" to="packet"/>
<remap from="~scan" to="/rear_scan"/>
<param name="device_ip" type="string" value="$(arg device_ip_2)"/>
<param name="device_port" type="int" value="$(arg device_port_2)"/>
<param name="local_ip" type="string" value="$(arg local_ip)"/>
<param name="multiaddr" type="string" value="$(arg multiaddr)"/>
</node> -->
</launch>

16
Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,16 @@
cmake_minimum_required(VERSION 2.8.3)
project(olei_msgs)
find_package(catkin REQUIRED COMPONENTS message_generation std_msgs)
add_message_files(
DIRECTORY msg
FILES
oleiPacket.msg
oleiScan.msg
)
generate_messages(DEPENDENCIES std_msgs)
catkin_package(
CATKIN_DEPENDS message_runtime std_msgs
)

5
Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiPacket.msg Executable file
View File

@@ -0,0 +1,5 @@
# Raw olei LIDAR packet.
time stamp # packet timestamp
uint8[1240] data # packet contents

4
Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/msg/oleiScan.msg Executable file
View File

@@ -0,0 +1,4 @@
# olei LIDAR scan packets.
Header header # standard ROS message header
oleiPacket[] packets # vector of raw packets

23
Devices/Packages/olei_lidar_driver_ros/olelidar/olei_msgs/package.xml Executable file
View File

@@ -0,0 +1,23 @@
<?xml version="1.0"?>
<package format="2">
<name>olei_msgs</name>
<version>1.5.2</version>
<description>
ROS message definitions for olei 2D LIDARs.
</description>
<maintainer email="emanuele.montemurro@olei.com">Emanuele</maintainer>
<author>Emanuele</author>
<license>BSD</license>
<url type="website">TODO</url>
<url type="repository">TODO</url>
<url type="bugtracker">TODO</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<depend>std_msgs</depend>
<exec_depend>message_runtime</exec_depend>
</package>

26
Devices/Packages/olei_lidar_driver_ros/olelidar/package.xml Executable file
View File

@@ -0,0 +1,26 @@
<package format="2">
<name>olelidar</name>
<version>3.0.0</version>
<description>
Basic ROS support for the olei 2D LIDARs.
</description>
<maintainer email="emanuele.montemurro@olei.com">Emanuele</maintainer>
<author>Emanuele</author>
<license>GNU General Public License V3.0</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>sensor_msgs</depend>
<depend>olei_msgs</depend>
<depend>roscpp</depend>
<depend>diagnostic_updater</depend>
<depend>dynamic_reconfigure</depend>
<export>
<nodelet plugin="${prefix}/nodelet_plugins.xml"/>
</export>
</package>

75
Devices/Packages/olei_lidar_driver_ros/olelidar/src/constants.h Executable file
View File

@@ -0,0 +1,75 @@
#pragma once
#include <cmath>
#include <cstdint>
#include <limits>
namespace olelidar {
static constexpr auto kNaNF = std::numeric_limits<float>::quiet_NaN();
static constexpr auto kNaND = std::numeric_limits<double>::quiet_NaN();
static constexpr float kTau = M_PI * 2;
static constexpr float deg2rad(float deg) { return deg * M_PI / 180.0; }
static constexpr float rad2deg(float rad) { return rad * 180.0 / M_PI; }
// Raw olei packet constants and structures.
// 1 packet = 150 blocks
// 1 block = 1 sequence
// 1 sequence = 1 firing = 1 point
// 1 point = 8 bytes
static constexpr int kPointBytes = 8;
static constexpr int kPointsPerBlock = 1;
// According to Bruce Hall DISTANCE_MAX is 65.0, but we noticed
// valid packets with readings up to 130.0.
static constexpr float kDistanceMax = 20.0; // [m]
static constexpr float kDistanceResolution = 0.001; // [m] 2d lidar,
/** @todo make this work for both big and little-endian machines */
static const uint16_t UPPER_BANK = 0xeeff; //rsv
static const uint16_t LOWER_BANK = 0xddff; //rsv
/** Special Defines for olei support **/
static constexpr double kSingleFiringNs = 2304; // [ns] rsv,1s/10hz/1600points = 62500
static constexpr double kFiringCycleNs = 55296; // [ns] rsv, = 62500
static constexpr double kSingleFiringRatio = kSingleFiringNs / kFiringCycleNs; // 1
// The information from two firing sequences of 16 lasers is contained in each
// data block. Each packet contains the data from 24 firing sequences in 12 data
// blocks.
static constexpr int kFiringsPerSequence = 1;
static constexpr int kSequencesPerBlock = 1;
static constexpr int kBlocksPerPacket = 150;
static constexpr int kSequencesPerPacket =
kSequencesPerBlock * kBlocksPerPacket; // 150
inline int LaserId2Row(int id) {
const auto index = (id % 2 == 0) ? id / 2 : id / 2 + kFiringsPerSequence / 2;
return kFiringsPerSequence - index - 1;
}
static constexpr uint16_t kMaxRawAzimuth = 35999;
static constexpr float kAzimuthResolution = 0.01f;
// static constexpr float kMinElevation = deg2rad(-15.0f);
// static constexpr float kMaxElevation = deg2rad(15.0f);
// static constexpr float kDeltaElevation =
// (kMaxElevation - kMinElevation) / (kFiringsPerSequence - 1);
inline constexpr float Raw2Azimuth(uint16_t raw) {
// According to the user manual,
return deg2rad(static_cast<float>(raw) * kAzimuthResolution);
}
/// p55 9.3.1.2
inline constexpr float Raw2Distance(uint16_t raw) {
return static_cast<float>(raw) * kDistanceResolution;
}
/// p51 8.3.1
inline constexpr float AzimuthResolutionDegree(int rpm) {
// rpm % 60 == 0
return rpm / 60.0 * 360.0 * kFiringCycleNs / 1e9;
}
} // namespace olelidar

446
Devices/Packages/olei_lidar_driver_ros/olelidar/src/decoder.cpp Executable file
View File

@@ -0,0 +1,446 @@
#include "constants.h"
#include <dynamic_reconfigure/server.h>
#include <sensor_msgs/LaserScan.h>
#include <olei_msgs/oleiPacket.h>
#include <olei_msgs/oleiScan.h>
#include <olelidar/oleiPuckConfig.h>
#include "driver.cpp"
// here maskoff waring of macro 'ROS_LOG'
#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant"
namespace olelidar
{
using namespace sensor_msgs;
using namespace olei_msgs;
class Decoder
{
public:
explicit Decoder(const ros::NodeHandle &pnh);
Decoder(const Decoder &) = delete;
Decoder operator=(const Decoder &) = delete;
void PacketCb(const oleiPacketConstPtr &packet_msg);
void ConfigCb(oleiPuckConfig &config, int level);
void PublishMsg(ros::Publisher *pub, std::vector<uint16_t> &packet_r, std::vector<uint16_t> &packet_i, ros::Time t);
private:
/// 9.3.1.3 Data Point
/// A data point is a measurement by one laser channel of a relection of a
/// laser pulse
struct DataPoint
{
uint16_t azimuth; //unit: 0.01 degree
uint16_t distance; //unit: 1mm
uint16_t reflectivity; //unit: 1 percent,100 is the reflet of white target
uint16_t distance2; //rsv for dual return
} __attribute__((packed));
static_assert(sizeof(DataPoint) == 8, "sizeof(DataPoint) != 8");
struct FiringSequence
{
DataPoint points[kFiringsPerSequence]; // 8
} __attribute__((packed));
static_assert(sizeof(FiringSequence) == 8, "sizeof(FiringSequence) != 8");
struct DataHead
{
uint8_t magic[4];
uint8_t version[2];
uint8_t scale;
uint8_t oem[3];
uint8_t model[12];
uint8_t code[2];
uint8_t hw[2];
uint8_t sw[2];
uint32_t timestamp;
uint16_t rpm;
uint8_t flag[2];
uint32_t rsv;
} __attribute__((packed));
static_assert(sizeof(DataHead) == 40, "sizeof(DataBlock) != 40");
struct DataBlock
{
FiringSequence sequences[kSequencesPerBlock]; // 8 * 1
} __attribute__((packed));
static_assert(sizeof(DataBlock) == 8, "sizeof(DataBlock) != 8");
/// 9.3.1.5 Packet format for 2d
struct Packet
{
DataHead head;
DataBlock blocks[kBlocksPerPacket]; // 8 * 150
} __attribute__((packed));
static_assert(sizeof(Packet) == 1240, "sizeof(DataBlock) != 1240");
static_assert(sizeof(Packet) == sizeof(olei_msgs::oleiPacket().data), "sizeof(Packet) != 1240");
void DecodeAndFill(const Packet *const packet_buf, uint64_t);
private:
bool CheckFactoryBytes(const Packet *const packet);
// ROS related parameters
uint32_t laststamp;
uint32_t scantime;
ros::Time start;
bool is_time_base_{false};
ros::Time local_timestamp_base_;
uint32_t inner_timestamp_base_;
std::string frame_id_;
int range_min_=200;
int range_max_;
int ange_start_;
int ange_end_;
bool inverted_;
int poly_=1;
ros::NodeHandle pnh_;
// sub driver topic(msg)
ros::Subscriber packet_sub_;
// pub laserscan message
ros::Publisher scan_pub_;
// dynamic param server
dynamic_reconfigure::Server<oleiPuckConfig> cfg_server_;
oleiPuckConfig config_;
// add vector for laserscan
std::vector<uint16_t> scanAngleVec_;
std::vector<uint16_t> scanRangeVec_;
std::vector<uint16_t> scanIntensityVec_;
std::vector<uint16_t> scanAngleInVec_;
std::vector<uint16_t> scanRangeInVec_;
std::vector<uint16_t> scanIntensityInVec_;
std::vector<float> scanRangeBuffer;
std::vector<float> scanintensitiesBuffer;
uint16_t azimuthLast_;
uint16_t azimuthNow_;
uint16_t azimuthFirst_;
// laserscan msg
uint32_t scanMsgSeq_;
//电机频率
float frequency;
//雷达型号类型
unsigned char lidarType=0x01;
//电机方向定义
int direction;
ros::Time log_time_ = ros::Time::now();
std::shared_ptr<Driver> drv_;
};
Decoder::Decoder(const ros::NodeHandle &pnh)
: pnh_(pnh), cfg_server_(pnh)
{
// get param from cfg file at node start
pnh_.param<std::string>("frame_id", frame_id_, "olelidar");
pnh_.param<int>("r_max", range_max_, 30);
ROS_INFO("===========================");
ROS_INFO("Frame_id: %s", frame_id_.c_str());
ROS_INFO("Topic: /%s/scan", frame_id_.c_str());
ROS_INFO("Range: [%d ~ %d] mm",range_min_, range_max_*1000);
ROS_INFO("===========================");
start = ros::Time::now();
// dynamic callback
cfg_server_.setCallback(boost::bind(&Decoder::ConfigCb, this, _1, _2));
// packet receive
azimuthLast_ = 0;
azimuthNow_ = 0;
azimuthFirst_ = 0xFFFF;
// laserscan msg init
scanMsgSeq_ = 0;
direction = 0;
drv_ = std::make_shared<Driver>(pnh);
scan_pub_ = pnh_.advertise<LaserScan>("scan", 100);
#ifdef DRIVER_MODULE
packet_sub_ = pnh_.subscribe<oleiPacket>("packet", 100, &Decoder::PacketCb, this, ros::TransportHints().tcpNoDelay(true));
#endif
drv_->setCallback(std::bind(&Decoder::PacketCb, this, std::placeholders::_1));
ROS_INFO("Drive Ver:2.0.11");
ROS_INFO("Decoder initialized");
}
void Decoder::PublishMsg(ros::Publisher *pub, std::vector<uint16_t> &packet_r, std::vector<uint16_t> &packet_i, ros::Time lidar_time)
{
sensor_msgs::LaserScan scanMsg;
/*
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
*/
int min = config_.angle_min * 100 + 18000;
int max = config_.angle_max * 100 + 18000;
scanRangeBuffer.clear();
scanintensitiesBuffer.clear();
for (uint16_t i = 0; i < scanAngleInVec_.size(); i++)
{
// 过滤出指定角度范围内点云
int angle = scanAngleInVec_[i];
if (angle >= min && angle <= max && i % poly_ == 0)
{
float range = scanRangeInVec_[i] * 0.001f;
float intensities = scanIntensityInVec_[i] * 1.0f;
scanRangeBuffer.push_back(range);
scanintensitiesBuffer.push_back(intensities);
}
}
float bufferlen = scanRangeBuffer.size();
scanMsg.ranges.resize(bufferlen);
scanMsg.intensities.resize(bufferlen);
if(direction ==0) //电机旋转顺时针时，转换右手坐标系法则
{
reverse(scanRangeBuffer.begin(),scanRangeBuffer.end());
reverse(scanintensitiesBuffer.begin(),scanintensitiesBuffer.end());
}
for (uint16_t i = 0; i < bufferlen; i++)
{
scanMsg.ranges[i] = scanRangeBuffer[i];
scanMsg.intensities[i] = scanintensitiesBuffer[i];
}
float len = scanMsg.ranges.size();
//扫描顺序自增ID序列
scanMsg.header.seq = scanMsgSeq_;
scanMsgSeq_++;
//激光数据时间戳
//scanMsg.header.stamp = lidar_time;
scanMsg.header.stamp = ros::Time::now();
//FrameId
scanMsg.header.frame_id = frame_id_.c_str();
//定义开始角度和结束角度
scanMsg.angle_min = deg2rad(config_.angle_min);
scanMsg.angle_max = deg2rad(config_.angle_max);
//定义测距的最小值和最大值单位:m
scanMsg.range_min = config_.range_min;
scanMsg.range_max = config_.range_max;
float step=(config_.angle_max - config_.angle_min)/len;
//ROS_INFO("len:%f step:%f",len,step);
//角度分辨率
scanMsg.angle_increment = deg2rad(step);
//扫描的时间间隔
scanMsg.scan_time = 1/frequency;
//时间分辨率（相邻两个角度之间耗费时间）
scanMsg.time_increment = scanMsg.scan_time/float(len);
//修正为雷达内部时钟间隔
//scanMsg.scan_time = scantime*0.001f; //雷达时间戳为毫秒计数，故而转为秒单位应乘以0.001
//scanMsg.time_increment = scanMsg.scan_time/float(len);
//ROS_INFO("scan_time:%f time_increment:%f",scanMsg.scan_time,scanMsg.time_increment);
uint16_t size=scanAngleInVec_.size();
uint16_t fb=360/(config_.step);
if (fb==size){
pub->publish(scanMsg); //校验当符合点数完整的一帧数据才向外发布话题
}
else{
if(scanMsgSeq_==1) return;
ROS_INFO("pointCloud frame:%d size:%d scanMsgSeq_:%d",fb,size,scanMsgSeq_);
}
}
void Decoder::DecodeAndFill(const Packet *const packet_buf, uint64_t time)
{
// For each data block, 150 total
uint16_t azimuth;
uint16_t range;
uint16_t intensity;
for (int iblk = 0; iblk < kBlocksPerPacket; ++iblk)
{
const auto &block = packet_buf->blocks[iblk];
// simple loop
azimuth = block.sequences[0].points[0].azimuth;
range = block.sequences[0].points[0].distance;
intensity = block.sequences[0].points[0].reflectivity;
//排除异常点云数据
if (range > range_max_*1000 || range < range_min_)
{
range = 0;
intensity = 0;
}
// azimuth ge 36000 is not valid
if (azimuth < 0xFF00)
{
scanAngleVec_.push_back(azimuth);
scanRangeVec_.push_back(range);
scanIntensityVec_.push_back(intensity);
}
}
}
void Decoder::PacketCb(const oleiPacketConstPtr &packet_msg)
{
ros::Time now = ros::Time::now();
ros::Duration delta_t = now - log_time_;
log_time_ = now;
const auto *packet_buf = reinterpret_cast<const Packet *>(&(packet_msg->data[0]));
azimuthNow_ = packet_buf->blocks[0].sequences[0].points[0].azimuth;
//取得第一个数据包
if (azimuthFirst_ == 0xFFFF)
{
//雷达型号类型
lidarType = packet_buf->head.code[1];
azimuthFirst_ = azimuthNow_;
//取得转速
int rpm = (packet_buf->head.rpm) & 0x7FFF;
//取得电机旋转方向
direction = (packet_buf->head.rpm) >> 15;
ROS_INFO("rpm:%d direction:%d lidarType:%d", rpm, direction,lidarType);
//是否启用倒装设定
if (inverted_) direction = !direction;
}
if (azimuthLast_ < azimuthNow_)
{
DecodeAndFill(packet_buf, packet_msg->stamp.toNSec());
azimuthLast_ = azimuthNow_;
return;
}
else
{
azimuthLast_ = azimuthNow_;
}
// scan first half route
if (azimuthFirst_ >= 200)
{
azimuthFirst_ = azimuthNow_;
return;
}
//雷达时间戳
uint32_t nowstamp = packet_buf->head.timestamp;
ros::Time lidar_time = packet_msg->stamp;
#ifdef TIMESTAMP_DEBUG
if(!is_time_base_) {
local_timestamp_base_ = ros::Time::now();
inner_timestamp_base_ = nowstamp;
lidar_time = local_timestamp_base_;
is_time_base_ = true;
} else {
uint32_t delta_time = nowstamp - inner_timestamp_base_;
ros::Duration dur_time;
ros::Duration delta_t = dur_time.fromNSec(delta_time*1000000);
lidar_time = local_timestamp_base_ + delta_t;
//ROS_INFO_STREAM("ros timestamp:" << delta_time << "," << delta_t << "," << lidar_time);
}
#endif
scantime = nowstamp - laststamp;
laststamp = nowstamp;
//ROS_INFO("inner timestamp: %u, %d", nowstamp, scantime);
//雷达工作频率
if(frequency<0.001){
lidarType = packet_buf->head.code[1]; //雷达型号类型
if(lidarType==0x01){
int rpm = (packet_buf->head.rpm) & 0x7FFF;
frequency = rpm / 60.0;
config_.step=0.225; //当雷达型号为0x01类型时，角分辨率为固定值
}
else{
if(scanAngleVec_.size()>2) {
//角度分辨率
config_.step = (scanAngleVec_[1]-scanAngleVec_[0])/100.0;
frequency = config_.step * 10000.0/60.0;
}
else{
return;
}
}
ROS_INFO("frequency: %.0f hz config_.step:%f",frequency,config_.step);
}
//当启用NTP服务时，时间戳重定向为NTP服务时间
if(packet_buf->head.rsv>0){
ros::Time ntp(packet_buf->head.rsv, packet_buf->head.timestamp);
lidar_time=ntp;
}
scanAngleInVec_.clear();
scanRangeInVec_.clear();
scanIntensityInVec_.clear();
scanAngleInVec_.assign(scanAngleVec_.begin(), scanAngleVec_.end());
scanRangeInVec_.assign(scanRangeVec_.begin(), scanRangeVec_.end());
scanIntensityInVec_.assign(scanIntensityVec_.begin(), scanIntensityVec_.end());
scanAngleVec_.clear();
scanRangeVec_.clear();
scanIntensityVec_.clear();
//抛出点云数据
PublishMsg(&scan_pub_, scanRangeInVec_, scanIntensityInVec_, lidar_time);
//解码原始数据包
DecodeAndFill(packet_buf, packet_msg->stamp.toNSec());
//ROS_INFO("Time: %f", (ros::Time::now() - start).toSec());
}
void Decoder::ConfigCb(oleiPuckConfig &config, int level)
{
// config.min_range = std::min(config.min_range, config.max_range);
//config.route =4000;
pnh_.param<int>("ang_start", ange_start_, 0);
pnh_.param<int>("ang_end", ange_end_, 360);
pnh_.param<int>("r_max", range_max_, 50);
pnh_.param<bool>("inverted", inverted_, false);
config.angle_min = ange_start_;
config.angle_max = ange_end_;
config.range_min = range_min_*0.001;
config.range_max = range_max_;
config_ = config;
if (level < 0){
// topic = scan, msg = LaserScan, queuesize = 1000
scan_pub_ = pnh_.advertise<LaserScan>("scan", 100);
#ifdef DRIVER_MODULE
packet_sub_ = pnh_.subscribe<oleiPacket>("packet", 100, &Decoder::PacketCb, this);
#endif
}
}
} // namespace olelidar
int main(int argc, char **argv)
{
ros::init(argc, argv, "olelidar_decoder");
ros::NodeHandle pnh("~");
olelidar::Decoder node(pnh);
ros::spin();
}

285
Devices/Packages/olei_lidar_driver_ros/olelidar/src/driver.cpp Executable file
View File

@@ -0,0 +1,285 @@
#include <arpa/inet.h>
#include <errno.h>
#include <fcntl.h>
#include <netinet/in.h>
#include <poll.h>
#include <sys/file.h>
#include <sys/socket.h>
#include <unistd.h>
#include <cmath>
#include <functional>
#include <thread>
#include "constants.h"
#include <diagnostic_updater/diagnostic_updater.h>
#include <diagnostic_updater/publisher.h>
#include <olei_msgs/oleiPacket.h>
#include <olei_msgs/oleiScan.h>
// here maskoff waring of macro 'ROS_LOG'
#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant"
namespace olelidar
{
using namespace olei_msgs;
using namespace diagnostic_updater;
/// Constants
//static constexpr uint16_t kUdpPort = 2368;
static constexpr size_t kPacketSize = sizeof(oleiPacket().data);
static constexpr int kError = -1;
class Driver
{
using data_cb_t = std::function<void(const oleiPacketConstPtr&)>;
public:
explicit Driver(const ros::NodeHandle &pnh);
~Driver();
bool Poll();
void setCallback(const data_cb_t &cb) { data_cb_ = cb; }
private:
bool OpenUdpPort();
int ReadPacket(oleiPacket &packet);
// Ethernet relate variables
std::string device_ip_str_;
std::string local_ip_str_;
std::string multiaddr_ip_str_;
int device_port_;
in_addr device_ip_;
int socket_id_{-1};
ros::Time last_time_;
// ROS related variables
ros::NodeHandle pnh_;
ros::Publisher pub_packet_;
// Diagnostics updater
diagnostic_updater::Updater updater_;
boost::shared_ptr<diagnostic_updater::TopicDiagnostic> topic_diag_;
std::vector<oleiPacket> buffer_;
// double freq_;
// raw data callback
data_cb_t data_cb_{nullptr};
std::thread data_thr_;
bool is_loop_{false};
};
Driver::Driver(const ros::NodeHandle &pnh)
: pnh_(pnh)
, last_time_(ros::Time::now())
{
ROS_INFO("packet size: %zu", kPacketSize);
pnh_.param("device_ip", device_ip_str_, std::string("192.168.1.122"));
pnh_.param("device_port", device_port_, 2369);
pnh_.param("local_ip", local_ip_str_, std::string("192.168.1.99"));
pnh_.param("multiaddr", multiaddr_ip_str_, std::string(""));
ROS_INFO("device_ip: %s:%d", device_ip_str_.c_str(), device_port_);
if (inet_aton(device_ip_str_.c_str(), &device_ip_) == 0)
{
// inet_aton() returns nonzero if the address is valid, zero if not.
ROS_FATAL("Invalid device ip: %s:%d", device_ip_str_.c_str(), device_port_);
ros::shutdown();
}
// Output
pub_packet_ = pnh_.advertise<oleiPacket>("packet", 10);
if (!OpenUdpPort()) ROS_ERROR("Failed to open UDP Port");
data_thr_ = std::move(std::thread( [&] {
is_loop_ = true;
while (is_loop_) {
if(!is_loop_) break;
Poll();
}
}));
ROS_INFO("Successfully init olelidar driver");
}
Driver::~Driver()
{
is_loop_ = false;
if(data_thr_.joinable()) {
data_thr_.join();
}
if (close(socket_id_)) {
ROS_INFO("Close socket %d at %s", socket_id_, device_ip_str_.c_str());
}
else {
ROS_ERROR("Failed to close socket %d at %s", socket_id_,device_ip_str_.c_str());
}
}
bool Driver::OpenUdpPort()
{
socket_id_ = socket(AF_INET, SOCK_DGRAM, 0);
if (socket_id_ == -1) {
perror("socket");
ROS_ERROR("Failed to create socket");
return false;
}
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(uint16_t(device_port_)); // short, in network byte order
my_addr.sin_addr.s_addr = INADDR_ANY; // automatically fill in my IP
if (bind(socket_id_, (sockaddr *)&my_addr, sizeof(sockaddr)) == -1)
{
perror("bind"); // TODO: ROS_ERROR errno
ROS_ERROR("Failed to bind to socket %d", socket_id_);
return false;
}
if(multiaddr_ip_str_!="")
{
//加入组播才能接受到组播信息
struct ip_mreq mreq;
mreq.imr_multiaddr.s_addr = inet_addr(multiaddr_ip_str_.c_str());
mreq.imr_interface.s_addr = inet_addr(local_ip_str_.c_str());
int err=setsockopt(socket_id_,IPPROTO_IP,IP_ADD_MEMBERSHIP,&mreq,sizeof(mreq));
ROS_INFO("AddMultiaddr:%s local:%s =>%d ",multiaddr_ip_str_.c_str(),local_ip_str_.c_str(),err);
}
if (fcntl(socket_id_, F_SETFL, O_NONBLOCK | FASYNC) < 0)
{
perror("non-block");
ROS_ERROR("Failed to set socket to non-blocking");
return false;
}
return true;
}
int Driver::ReadPacket(oleiPacket &packet)
{
ros::Time time_before = ros::Time::now();
struct pollfd fds[1];
fds[0].fd = socket_id_;
fds[0].events = POLLIN;
const int timeout_ms = 500; // one second (in msec)
sockaddr_in sender_address;
socklen_t sender_address_len = sizeof(sender_address);
while (true)
{
do
{
const int retval = poll(fds, 1, timeout_ms);
if (retval < 0) {
if (errno != EINTR) ROS_ERROR("poll() error: %s", strerror(errno));
return kError;
}
else if (retval == 0)
{
ROS_WARN("olamlidar poll() timeout");
return kError;
}
if ((fds[0].revents & POLLERR) || (fds[0].revents & POLLHUP) || (fds[0].revents & POLLNVAL))
{
ROS_ERROR("poll() reports olamlidar error");
return kError;
}
} while ((fds[0].revents & POLLIN) == 0);
// Receive packets that should now be available from the
// socket using a blocking read.
const ssize_t nbytes = recvfrom(socket_id_, &packet.data[0], kPacketSize, 0, (sockaddr *)&sender_address, &sender_address_len);
if (nbytes < 0)
{
if (errno != EWOULDBLOCK)
{
perror("recvfail");
ROS_ERROR("Failed to read from socket");
return kError;
}
}
else if (static_cast<size_t>(nbytes) == kPacketSize)
{
//else if ((size_t)nbytes == kPacketSize) {
// read successful,
// if packet is not from the lidar scanner we selected by IP,
// continue otherwise we are done
//ROS_ERROR("Failed to read from socket");
//ROS_INFO("sender:%d device:%d",sender_address.sin_addr.s_addr,device_ip_.s_addr);
if (device_ip_str_ != "" && sender_address.sin_addr.s_addr != device_ip_.s_addr)
continue;
else
break; // done
}
ROS_DEBUG_STREAM("incomplete olei packet read: " << nbytes << " bytes");
}
packet.stamp = time_before;
#ifdef TIMESTAMP_DEBUG
ros::Duration delta = time_before - last_time_;
ROS_INFO_STREAM("raw data delta time: " << time_before << "," << delta.toSec()*1000);
last_time_ = time_before;
#endif
return 0;
}
bool Driver::Poll()
{
oleiPacket::Ptr packet(new oleiPacket);
while (true)
{
// keep reading until full packet received
const int rc = ReadPacket(*packet);
if (rc == 0)
break; // got a full packet?
if (rc < 0)
return false; // end of file reached?
}
// publish message using time of last packet read
#ifdef DRIVER_MODULE
pub_packet_.publish(packet);
#else
data_cb_(packet);
#endif
return true;
}
} // namespace olelidar
#ifdef DRIVER_MODULE
int main(int argc, char **argv)
{
ros::init(argc, argv, "olelidar_driver");
ros::NodeHandle pnh("~");
olelidar::Driver node(pnh);
while (ros::ok())
{
// poll device until end of file
if (!node.Poll())
{
ROS_WARN("Failed to poll device");
//break;
}
ros::spinOnce();
}
return 0;
}
#endif