Skip to content

Commit

Permalink
[update]版本号v2.2.10,更改点如下：
Browse files Browse the repository at this point in the history 
1. 保留原始数据中不良点，其距离和强度置为0，发布时置为nan
2. 优化串口回调函数调用方式
3. 增加互斥信号量，防止多线程间共享资源竞争
  • Loading branch information
@ldrobotsensor
ldrobotsensor committed May 13, 2022
1 parent 78da1d3 commit 1ebb0f1
Show file tree
Hide file tree
Showing 10 changed files with 171 additions and 99 deletions.
3 changes: 2 additions & 1 deletion .gitignore
View file
Original file line number Diff line number Diff line change
@@ -1,2 +1,3 @@
.vscode
*.zip
*.zip
core
10 changes: 5 additions & 5 deletions listen/listen_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,10 @@

void LidarMsgCallback(const sensor_msgs::LaserScan::ConstPtr& data)
{
unsigned int lens = (data->angle_max - data->angle_min) / data->angle_increment;
unsigned int lens = static_cast<unsigned int>((data->angle_max - data->angle_min) / data->angle_increment);
ROS_INFO_STREAM("[ldrobot] angle_min: " << RADIAN_TO_DEGREES(data->angle_min) << " "
<< "angle_max: " << RADIAN_TO_DEGREES(data->angle_max));

ROS_INFO_STREAM("[ldrobot] point size: " << data->ranges.size());
for (unsigned int i = 0; i < lens; i++) {
ROS_INFO_STREAM("[ldrobot] angle: " << RADIAN_TO_DEGREES((data->angle_min + i * data->angle_increment)) << " "
<< "range: " << data->ranges[i] << " "
Expand All @@ -50,13 +50,13 @@ int main(int argc, char **argv)
n.getParam("topic_name", topic_name);

if (topic_name.empty()) {
ROS_ERROR("[ldrobot] input param <topic_name> is null");
ROS_ERROR("[ldrobot] [ldldiar_listen_node] input param <topic_name> is null");
exit(EXIT_FAILURE);
} else {
ROS_INFO("[ldrobot] input param <topic_name> is %s", topic_name.c_str());
ROS_INFO("[ldrobot] [ldldiar_listen_node] input param <topic_name> is %s", topic_name.c_str());
}

ros::Subscriber msg_subs = nh.subscribe(topic_name, 1, &LidarMsgCallback);
ros::Subscriber msg_subs = nh.subscribe(topic_name, 10, &LidarMsgCallback);
ROS_INFO("[ldrobot] start ldldiar message subscribe node");

ros::Rate loop_rate(10);
Expand Down
2 changes: 1 addition & 1 deletion package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
<maintainer email="marketing1@ldrobot.com">marketing1</maintainer>
<maintainer email="support@ldrobot.com">marketing1</maintainer>


<!-- One license tag required, multiple allowed, one license per tag -->
Expand Down
18 changes: 9 additions & 9 deletions rviz/ldlidar_kinetic_melodic.rviz
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ Panels:
- /Status1
- /LaserScan1
Splitter Ratio: 0.5
Tree Height: 565
Tree Height: 613
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expand Down Expand Up @@ -93,9 +93,9 @@ Visualization Manager:
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 245
Max Intensity: 249
Min Color: 0; 0; 0
Min Intensity: 22
Min Intensity: 35
Name: LaserScan
Position Transformer: XYZ
Queue Size: 10
Expand Down Expand Up @@ -133,7 +133,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 7.74400043
Distance: 26.3351135
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Expand All @@ -156,10 +156,10 @@ Visualization Manager:
Window Geometry:
Displays:
collapsed: false
Height: 846
Height: 894
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
QMainWindow State: 000000ff00000000fd00000004000000000000016a000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002f4000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002f4000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007450000003efc0100000002fb0000000800540069006d00650100000000000007450000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004c0000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
Expand All @@ -168,6 +168,6 @@ Window Geometry:
collapsed: false
Views:
collapsed: false
Width: 1200
X: 344
Y: 32
Width: 1861
X: 57
Y: 24
70 changes: 52 additions & 18 deletions src/lipkg.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -67,15 +67,13 @@ LiPkg::LiPkg(std::string frame_id, bool laser_scan_dir, bool enable_angle_crop_f
speed_(0),
error_times_(0),
is_frame_ready_(false),
is_pkg_ready_(false),
frame_id_(frame_id),
laser_scan_dir_(laser_scan_dir),
enable_angle_crop_func_(enable_angle_crop_func),
angle_crop_min_(angle_crop_min),
angle_crop_max_(angle_crop_max) {
frame_id_ = frame_id;
}

double LiPkg::GetSpeed(void) { return speed_ / 360.0; }
}

bool LiPkg::AnalysisOne(uint8_t byte) {
static enum {
Expand Down Expand Up @@ -148,12 +146,8 @@ bool LiPkg::Parse(const uint8_t *data, long len) {
data.angle -= 360.0;
}
data.intensity = pkg.point[i].intensity;
one_pkg_[i] = data;
frame_tmp_.push_back(PointData(data.angle, data.distance, data.intensity));
}
// prevent angle invert
one_pkg_.back().angle = end;
is_pkg_ready_ = true;
}
}
}
Expand All @@ -170,20 +164,21 @@ bool LiPkg::AssemblePacket() {
// wait for enough data, need enough data to show a circle
// enough data has been obtained
if ((n.angle < 20.0) && (last_angle > 340.0)) {
// std::cout << "count: " << count << std::endl;
if ((count * GetSpeed()) > (kPointFrequence * 1.4)) {
frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count - 1);
return false;
}
data.insert(data.begin(), frame_tmp_.begin(), frame_tmp_.begin() + count);
// std::cout << "data.size() " << data.size() << std::endl;
// ROS_INFO_STREAM("[ldrobot] filter front poit size: " << data.size());
Tofbf tofbfLd06(speed_);
tmp = tofbfLd06.NearFilter(data);

std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) { return a.angle < b.angle; });
// ROS_INFO_STREAM("[ldrobot] filter back poit size: " << tmp.size());
if (tmp.size() > 0) {
ToLaserscan(tmp);
is_frame_ready_ = true;
if (tmp.front().angle < 1.0) {
ToLaserscan(tmp);
SetFrameReady();
}
frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count);
return true;
}
Expand All @@ -195,20 +190,53 @@ bool LiPkg::AssemblePacket() {
return false;
}

const std::array<PointData, POINT_PER_PACK>& LiPkg::GetPkgData(void) {
is_pkg_ready_ = false;
return one_pkg_;
double LiPkg::GetSpeed(void) {
return (speed_ / 360.0); // unit is hz
}

void LiPkg::ToLaserscan(std::vector<PointData> src) {
float angle_min, angle_max, range_min, range_max, angle_increment;
uint16_t LiPkg::GetTimestamp(void) {
return timestamp_;
}

bool LiPkg::IsFrameReady(void) {
return is_frame_ready_;
}

void LiPkg::ResetFrameReady(void) {
{
std::lock_guard<std::mutex> lock(mutex_lock);
is_frame_ready_ = false;
}
}

void LiPkg::SetFrameReady(void) {
{
std::lock_guard<std::mutex> lock(mutex_lock);
is_frame_ready_ = true;
}
}

long LiPkg::GetErrorTimes(void) {
return error_times_;
}

void LiPkg::CommReadCallback(const char *byte, size_t len) {
if (this->Parse((uint8_t *)byte, len)) {
this->AssemblePacket();
}
}

void LiPkg::ToLaserscan(std::vector<PointData>& src) {
float angle_min, angle_max, range_min, range_max, angle_increment;
// Adjust the parameters according to the demand
angle_min = ANGLE_TO_RADIAN(src.front().angle);
angle_max = ANGLE_TO_RADIAN(src.back().angle);
range_min = 0.02;
range_max = 12;
angle_increment = ANGLE_TO_RADIAN(speed_ / kPointFrequence);
// ROS_INFO_STREAM("[ldrobot] angle min: " << src.front().angle);
// ROS_INFO_STREAM("[ldrobot] angle max: " << src.back().angle);
// ROS_INFO_STREAM("[ldrobot] speed(hz): " << GetSpeed());
static uint16_t last_times_stamp = 0;
float dealt_times_stamp = 0;
uint16_t tmp_times_stamp = GetTimestamp();
Expand All @@ -221,6 +249,7 @@ void LiPkg::ToLaserscan(std::vector<PointData> src) {
// Calculate the number of scanning points
if (speed_ > 0) {
unsigned int beam_size = static_cast<unsigned int>(ceil((angle_max - angle_min) / angle_increment));
// ROS_INFO_STREAM("[ldrobot] beam_size: " << beam_size);
output_.header.stamp = ros::Time::now();
output_.header.frame_id = frame_id_;
output_.angle_min = angle_min;
Expand All @@ -244,6 +273,11 @@ void LiPkg::ToLaserscan(std::vector<PointData> src) {
float intensity = point.intensity; // laser receive intensity
float dir_angle;

if ((point.distance == 0) && (point.intensity == 0)) { // filter is handled to 0, Nan will be assigned variable.
range = std::numeric_limits<float>::quiet_NaN();
intensity = std::numeric_limits<float>::quiet_NaN();
}

if (laser_scan_dir_) {
dir_angle = static_cast<float>(360.f - point.angle); // Lidar rotation data flow changed from clockwise to counterclockwise
} else {
Expand Down
35 changes: 17 additions & 18 deletions src/lipkg.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,14 @@
#ifndef __LIPKG_H
#define __LIPKG_H

#include <sensor_msgs/LaserScan.h>
#include "ros_api.h"
#include <stdint.h>

#include <array>
#include <iostream>
#include <vector>
#include <mutex>
#include <functional>

#include "pointdata.h"

Expand Down Expand Up @@ -60,22 +62,15 @@ class LiPkg {
// get Lidar spin speed (Hz)
double GetSpeed(void);
// get time stamp of the packet
uint16_t GetTimestamp(void) { return timestamp_; }
// a packet is ready
bool IsPkgReady(void) { return is_pkg_ready_; }
uint16_t GetTimestamp(void);
// Get lidar data frame ready flag
bool IsFrameReady(void) { return is_frame_ready_; }
bool IsFrameReady(void);
// Lidar data frame readiness flag reset
void ResetFrameReady(void) { is_frame_ready_ = false; }
void ResetFrameReady(void);
void SetFrameReady(void);
// the number of errors in parser process of lidar data frame
long GetErrorTimes(void) { return error_times_; }
// Get original Lidar data package
const std::array<PointData, POINT_PER_PACK>& GetPkgData(void);
// parse single packet
bool AnalysisOne(uint8_t byte);
bool Parse(const uint8_t* data, long len);
// combine stantard data into data frames and calibrate
bool AssemblePacket();
long GetErrorTimes(void);
void CommReadCallback(const char *byte, size_t len);
// Get sensor_msg/LaserScan type data
sensor_msgs::LaserScan GetLaserScan() { return output_; }

Expand All @@ -85,19 +80,23 @@ class LiPkg {
uint16_t timestamp_;
double speed_;
long error_times_;
bool is_pkg_ready_;
bool is_frame_ready_;
bool laser_scan_dir_;
bool enable_angle_crop_func_;
double angle_crop_min_;
double angle_crop_max_;
LiDARFrameTypeDef pkg;
std::vector<uint8_t> data_tmp_;
std::array<PointData, POINT_PER_PACK> one_pkg_;
std::vector<PointData> frame_tmp_;
std::mutex mutex_lock;
sensor_msgs::LaserScan output_;
// Lidar frame data tranfrom to sensor_msg/LaserScan type data
void ToLaserscan(std::vector<PointData> src);

// parse single packet
bool AnalysisOne(uint8_t byte);
bool Parse(const uint8_t* data, long len);
// combine stantard data into data frames and calibrate
bool AssemblePacket();
void ToLaserscan(std::vector<PointData>& src);
};

#endif //__LIPKG_H
Expand Down
36 changes: 15 additions & 21 deletions src/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@
* limitations under the License.
*/

#include <ros/ros.h>
#include "ros_api.h"
#include <stdio.h>

#include <iostream>
Expand All @@ -31,7 +31,7 @@
int main(int argc, char **argv) {
ros::init(argc, argv, "ldldiar_publisher");
ros::NodeHandle nh; // create a ROS Node
ros::NodeHandle n("~");
ros::NodeHandle nh_private("~");
std::string product_name;
std::string topic_name;
std::string port_name;
Expand All @@ -41,16 +41,16 @@ int main(int argc, char **argv) {
double angle_crop_min = 0.0;
double angle_crop_max = 0.0;

n.getParam("product_name", product_name);
n.getParam("topic_name", topic_name);
n.getParam("port_name", port_name);
n.getParam("frame_id", frame_id);
n.getParam("laser_scan_dir", laser_scan_dir);
n.getParam("enable_angle_crop_func", enable_angle_crop_func);
n.getParam("angle_crop_min", angle_crop_min);
n.getParam("angle_crop_max", angle_crop_max);
nh_private.getParam("product_name", product_name);
nh_private.getParam("topic_name", topic_name);
nh_private.getParam("port_name", port_name);
nh_private.getParam("frame_id", frame_id);
nh_private.getParam("laser_scan_dir", laser_scan_dir);
nh_private.getParam("enable_angle_crop_func", enable_angle_crop_func);
nh_private.getParam("angle_crop_min", angle_crop_min);
nh_private.getParam("angle_crop_max", angle_crop_max);

ROS_INFO("[ldrobot] SDK Pack Version is v2.2.9");
ROS_INFO("[ldrobot] SDK Pack Version is v2.2.10");
ROS_INFO("[ldrobot] <product_name>: %s,<topic_name>: %s,<port_name>: %s,<frame_id>: %s",
product_name.c_str(), topic_name.c_str(), port_name.c_str(), frame_id.c_str());

Expand All @@ -65,29 +65,23 @@ int main(int argc, char **argv) {
exit(EXIT_FAILURE);
} else {
ROS_INFO("[ldrobot] FOUND %s device", product_name.c_str());
cmd_port.SetReadCallback([&lidar](const char *byte, size_t len) {
if (lidar->Parse((uint8_t *)byte, len)) {
lidar->AssemblePacket();
}
});
cmd_port.SetReadCallback(std::bind(&LiPkg::CommReadCallback, lidar, std::placeholders::_1, std::placeholders::_2));
}

if (cmd_port.Open(port_name)) {
ROS_INFO("[ldrobot] open %s device %s is success!", product_name.c_str(), port_name.c_str());
}else {
ROS_INFO("[ldrobot] open %s device %s is fail!", product_name.c_str(), port_name.c_str());
ROS_ERROR("[ldrobot] open %s device %s is fail!", product_name.c_str(), port_name.c_str());
exit(EXIT_FAILURE);
}



ros::Publisher lidar_pub = nh.advertise<sensor_msgs::LaserScan>(topic_name, 10); // create a ROS topic

ros::Rate r(10); //10hz

while (ros::ok()) {
if (lidar->IsFrameReady()) {
lidar_pub.publish(lidar->GetLaserScan()); // Fixed Frame: lidar_frame
lidar->ResetFrameReady();
lidar_pub.publish(lidar->GetLaserScan());
ROS_INFO_STREAM("[ldrobot] current_speed(Hz): " << lidar->GetSpeed());
ROS_INFO_STREAM("----^---^-----");
}
Expand Down
Loading

0 comments on commit 1ebb0f1

Please sign in to comment.