Skip to content

Commit

Permalink
[update]版本号v2.3.3,更改点为: 1. 修改输出SDK版本号的方式;
Browse files Browse the repository at this point in the history
2. 添加名空间、修改头文件包含使更具移植性;
3. 修改脚本
  • Loading branch information
ldrobotsensor committed May 29, 2022
1 parent 5bb853a commit 73cd4af
Show file tree
Hide file tree
Showing 9 changed files with 63 additions and 49 deletions.
15 changes: 4 additions & 11 deletions include/ldlidar_driver/cmd_interface_linux.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -19,15 +19,7 @@

#include "cmd_interface_linux.h"

#include <errno.h>
#include <fcntl.h>
#include <memory.h>
#include <string.h>
#include <sys/file.h>
#include <termios.h>
#include <unistd.h>

#include <iostream>
namespace ldlidar {

#define MAX_ACK_BUF_LEN 4096

Expand Down Expand Up @@ -55,8 +47,8 @@ bool CmdInterfaceLinux::Open(std::string &port_name) {
return false;
}

options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8 | CRTSCTS);
options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB | PARODD);
options.c_cflag |= (tcflag_t)(CLOCAL | CREAD | CS8);
options.c_cflag &= (tcflag_t) ~(CSTOPB | PARENB);
options.c_lflag &= (tcflag_t) ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL |
ISIG | IEXTEN); //|ECHOPRT
options.c_oflag &= (tcflag_t) ~(OPOST);
Expand Down Expand Up @@ -166,5 +158,6 @@ void CmdInterfaceLinux::RxThreadProc(void *param) {
delete[] rx_buf;
}

} // namespace ldlidar
/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
* FILE ********/
11 changes: 11 additions & 0 deletions include/ldlidar_driver/include/cmd_interface_linux.h
Original file line number Diff line number Diff line change
Expand Up @@ -21,8 +21,15 @@
#define __LINUX_SERIAL_PORT_H__

#include <inttypes.h>
#include <errno.h>
#include <fcntl.h>
#include <memory.h>
#include <string.h>
#include <sys/file.h>
#include <termios.h>
#include <unistd.h>

#include <iostream>
#include <atomic>
#include <condition_variable>
#include <functional>
Expand All @@ -31,6 +38,8 @@
#include <thread>
#include <vector>

namespace ldlidar {

class CmdInterfaceLinux {
public:
CmdInterfaceLinux();
Expand Down Expand Up @@ -59,6 +68,8 @@ class CmdInterfaceLinux {
static void RxThreadProc(void *param);
};

} // namespace ldlidar

#endif //__LINUX_SERIAL_PORT_H__

/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
Expand Down
19 changes: 9 additions & 10 deletions include/ldlidar_driver/include/lipkg.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,19 +22,13 @@
#ifndef __LIPKG_H
#define __LIPKG_H

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

#include <iostream>
#include <vector>
#include <mutex>
#include <functional>
#include <string>
#include <chrono>

#include "cmd_interface_linux.h"
#include "pointdata.h"
#include "tofbf.h"

namespace ldlidar {

enum {
PKG_HEADER = 0x54,
Expand Down Expand Up @@ -64,6 +58,8 @@ class LiPkg {

LiPkg();
~LiPkg();
// get sdk pack version Number
std::string GetSdkVersionNumber();
// get Lidar spin speed (Hz)
double GetSpeed(void);
// get lidar spind speed (degree per second) origin
Expand All @@ -81,6 +77,7 @@ class LiPkg {
Points2D GetLaserScanData(void);

private:
std::string sdk_pack_version_;
uint16_t timestamp_;
double speed_;
long error_times_;
Expand All @@ -97,9 +94,11 @@ class LiPkg {
bool Parse(const uint8_t* data, long len);
// combine stantard data into data frames and calibrate
bool AssemblePacket();
void FillLaserScanData(Points2D& src);
void SetLaserScanData(Points2D& src);
};

} // namespace ldlidar

#endif //__LIPKG_H

/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
Expand Down
8 changes: 6 additions & 2 deletions include/ldlidar_driver/include/pointdata.h
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,11 @@
#include <iostream>
#include <vector>

namespace ldlidar {

#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
#define RADIAN_TO_ANGLED(angle) ((angle)*180000 / 3141.59)

struct PointData {
// Polar coordinate representation
float angle; // Angle ranges from 0 to 359 degrees
Expand All @@ -52,8 +57,7 @@ struct PointData {

typedef std::vector<PointData> Points2D;

#define ANGLE_TO_RADIAN(angle) ((angle)*3141.59 / 180000)
#define RADIAN_TO_ANGLED(angle) ((angle)*180000 / 3141.59)
} // namespace ldlidar

#endif // _POINT_DATA_H_

Expand Down
8 changes: 6 additions & 2 deletions include/ldlidar_driver/include/tofbf.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,12 +22,14 @@
#ifndef __TOFBF_H_
#define __TOFBF_H_

#include <stdint.h>
#include <math.h>

#include <vector>
#include <algorithm>

#include "pointdata.h"

namespace ldlidar {

class Tofbf {
private:
const int kIntensityLow = 15; // Low intensity threshold
Expand All @@ -45,6 +47,8 @@ class Tofbf {
~Tofbf();
};

} // namespace ldlidar

#endif //__TOFBF_H_

/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
Expand Down
19 changes: 10 additions & 9 deletions include/ldlidar_driver/lipkg.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,7 @@

#include "lipkg.h"

#include <math.h>
#include <string.h>

#include <algorithm>

#include "tofbf.h"
namespace ldlidar {

static const uint8_t CrcTable[256] = {
0x00, 0x4d, 0x9a, 0xd7, 0x79, 0x34, 0xe3, 0xae, 0xf2, 0xbf, 0x68, 0x25,
Expand Down Expand Up @@ -62,7 +57,8 @@ uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) {
}

LiPkg::LiPkg()
: timestamp_(0),
: sdk_pack_version_("v2.3.3"),
timestamp_(0),
speed_(0),
error_times_(0),
is_frame_ready_(false){
Expand All @@ -73,6 +69,10 @@ LiPkg::~LiPkg() {

}

std::string LiPkg::GetSdkVersionNumber() {
return sdk_pack_version_;
}

bool LiPkg::AnalysisOne(uint8_t byte) {
static enum {
HEADER,
Expand Down Expand Up @@ -170,7 +170,7 @@ bool LiPkg::AssemblePacket() {
tmp = tofFilter.NearFilter(data);
std::sort(tmp.begin(), tmp.end(), [](PointData a, PointData b) { return a.angle < b.angle; });
if (tmp.size() > 0) {
FillLaserScanData(tmp);
SetLaserScanData(tmp);
SetFrameReady();
frame_tmp_.erase(frame_tmp_.begin(), frame_tmp_.begin() + count);
return true;
Expand Down Expand Up @@ -224,10 +224,11 @@ Points2D LiPkg::GetLaserScanData(void) {
return laser_scan_data_;
}

void LiPkg::FillLaserScanData(Points2D& src) {
void LiPkg::SetLaserScanData(Points2D& src) {
std::lock_guard<std::mutex> lg(mutex_lock2_);
laser_scan_data_ = src;
}

} // namespace ldlidar
/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
* FILE ********/
6 changes: 2 additions & 4 deletions include/ldlidar_driver/tofbf.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,10 +21,7 @@

#include "tofbf.h"

#include <math.h>

#include <algorithm>
#include <iostream>
namespace ldlidar {

/**
* @brief Construct a new Tofbf:: Tofbf object
Expand Down Expand Up @@ -145,5 +142,6 @@ std::vector<PointData> Tofbf::NearFilter(
return normal;
}

} // namespace ldlidar
/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF
* FILE ********/
23 changes: 13 additions & 10 deletions src/publish_node/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@
#include "ros_api.h"
#include "lipkg.h"

void ToLaserscanMessagePublish(Points2D& src, LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub);
void ToLaserscanMessagePublish(ldlidar::Points2D& src, ldlidar::LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub);

int main(int argc, char **argv) {
ros::init(argc, argv, "ldldiar_publisher");
Expand All @@ -40,22 +40,23 @@ int main(int argc, char **argv) {
nh_private.param("enable_angle_crop_func", setting.enable_angle_crop_func, bool(false));
nh_private.param("angle_crop_min", setting.angle_crop_min, double(0.0));
nh_private.param("angle_crop_max", setting.angle_crop_max, double(0.0));

ROS_INFO("[ldrobot] SDK Pack Version is v2.3.2");

ldlidar::LiPkg *lidar_commh = new ldlidar::LiPkg();
ldlidar::CmdInterfaceLinux *cmd_port = new ldlidar::CmdInterfaceLinux();

ROS_INFO_STREAM("[ldrobot] SDK Pack Version is: " << lidar_commh->GetSdkVersionNumber());
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(), setting.frame_id.c_str());

ROS_INFO("[ldrobot] <laser_scan_dir>: %s,<enable_angle_crop_func>: %s,<angle_crop_min>: %f,<angle_crop_max>: %f",
(setting.laser_scan_dir?"Counterclockwise":"Clockwise"), (setting.enable_angle_crop_func?"true":"false"), setting.angle_crop_min, setting.angle_crop_max);

LiPkg *lidar_commh = new LiPkg();
CmdInterfaceLinux *cmd_port = new CmdInterfaceLinux();

if (port_name.empty()) {
ROS_ERROR("[ldrobot] input <port_name> param is null");
exit(EXIT_FAILURE);
}
cmd_port->SetReadCallback(std::bind(&LiPkg::CommReadCallback, lidar_commh, std::placeholders::_1, std::placeholders::_2));

cmd_port->SetReadCallback(std::bind(&ldlidar::LiPkg::CommReadCallback, lidar_commh, 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());
Expand All @@ -67,16 +68,18 @@ int main(int argc, char **argv) {
ros::Publisher lidar_pub = nh.advertise<sensor_msgs::LaserScan>(topic_name, 10); // create a ROS topic

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

auto last_time = std::chrono::steady_clock::now();

while (ros::ok()) {
if (lidar_commh->IsFrameReady()) {
lidar_commh->ResetFrameReady();
last_time = std::chrono::steady_clock::now();
Points2D laserscandata = lidar_commh->GetLaserScanData();
ldlidar::Points2D laserscandata = lidar_commh->GetLaserScanData();
ToLaserscanMessagePublish(laserscandata, lidar_commh, setting, lidar_pub);
}

if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now()-last_time).count() > 1000) { // 数据发布超时或者串口设备拔出处理
if (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now()-last_time).count() > 1000) {
ROS_ERROR("[ldrobot] lidar pub data is time out, please check lidar device");
exit(EXIT_FAILURE);
}
Expand All @@ -93,7 +96,7 @@ int main(int argc, char **argv) {
return 0;
}

void ToLaserscanMessagePublish(Points2D& src, LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub) {
void ToLaserscanMessagePublish(ldlidar::Points2D& src, ldlidar::LiPkg* commpkg, LaserScanSetting& setting, ros::Publisher& lidarpub) {
float angle_min, angle_max, range_min, range_max, angle_increment;
float scan_time;
ros::Time start_scan_time;
Expand Down
3 changes: 2 additions & 1 deletion ws_deploy.sh
Original file line number Diff line number Diff line change
Expand Up @@ -15,7 +15,8 @@ else
cp ./launch ./${floder_name} -a
cp ./rviz ./${floder_name} -a
cp ./src ./${floder_name} -a
cp ./listen ./${floder_name} -a
cp ./include ./${floder_name} -a
cp ./scripts ./${floder_name} -a
zip -r ${floder_name}.zip ./${floder_name}
rm -rf ./${floder_name}/
echo "create ./${floder_name}.zip "
Expand Down

0 comments on commit 73cd4af

Please sign in to comment.