diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..5d7e729
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,2 @@
+.vscode
+*.zip
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index ca59dc4..8d94ee2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -137,10 +137,12 @@ ${catkin_INCLUDE_DIRS}
#endif()
file(GLOB MAIN_SRC ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
-
add_executable(${PROJECT_NAME}_node ${MAIN_SRC})
target_link_libraries(${PROJECT_NAME}_node pthread ${catkin_LIBRARIES})
+file(GLOB SRC ${CMAKE_CURRENT_SOURCE_DIR}/listen/*.cpp)
+add_executable(${PROJECT_NAME}_listen_node ${SRC})
+target_link_libraries(${PROJECT_NAME}_listen_node ${catkin_LIBRARIES})
## Declare a C++ library
# add_library(${PROJECT_NAME}
diff --git a/launch/ld06.launch b/launch/ld06.launch
index cb0d4f3..df505bb 100644
--- a/launch/ld06.launch
+++ b/launch/ld06.launch
@@ -1,8 +1,26 @@
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/launch/ld19.launch b/launch/ld19.launch
index 6925575..7817053 100644
--- a/launch/ld19.launch
+++ b/launch/ld19.launch
@@ -1,8 +1,26 @@
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/listen/listen_node.cpp b/listen/listen_node.cpp
new file mode 100644
index 0000000..9d44845
--- /dev/null
+++ b/listen/listen_node.cpp
@@ -0,0 +1,71 @@
+/**
+* @file listen_node.cpp
+* @author LDRobot (marketing1@ldrobot.com)
+* @brief
+* @version 0.1
+* @date 2022.04.08
+* @note
+* @copyright Copyright (c) 2020 SHENZHEN LDROBOT CO., LTD. All rights reserved.
+* Licensed under the MIT License (the "License");
+* you may not use this file except in compliance with the License.
+* You may obtain a copy of the License in the file LICENSE
+* Unless required by applicable law or agreed to in writing, software
+* distributed under the License is distributed on an "AS IS" BASIS,
+* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+* See the License for the specific language governing permissions and
+* limitations under the License.
+**/
+
+#include "listen_node.h"
+
+#include
+#include
+
+#include
+#include
+
+#define RADIAN_TO_DEGREES(angle) ((angle)*180000/3141.59)
+
+void LidarMsgCallback(const sensor_msgs::LaserScan::ConstPtr& data)
+{
+ unsigned int lens = (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));
+
+ 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] << " "
+ << "intensites: " << data->intensities[i]);
+ }
+ ROS_INFO_STREAM("----------------------------");
+}
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv, "ldldiar_listen_node");
+ ros::NodeHandle nh; // create a ROS Node
+ ros::NodeHandle n("~");
+ std::string topic_name;
+
+ n.getParam("topic_name", topic_name);
+
+ if (topic_name.empty()) {
+ ROS_ERROR("[ldrobot] input param is null");
+ exit(EXIT_FAILURE);
+ } else {
+ ROS_INFO("[ldrobot] input param is %s", topic_name.c_str());
+ }
+
+ ros::Subscriber msg_subs = nh.subscribe(topic_name, 1, &LidarMsgCallback);
+ ROS_INFO("[ldrobot] start ldldiar message subscribe node");
+
+ ros::Rate loop_rate(10);
+ while (ros::ok()) {
+ ros::spinOnce();
+ loop_rate.sleep();
+ }
+
+ return 0;
+}
+
+/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/
diff --git a/listen/listen_node.h b/listen/listen_node.h
new file mode 100644
index 0000000..bf4dd29
--- /dev/null
+++ b/listen/listen_node.h
@@ -0,0 +1,19 @@
+/**
+* @file listen_node.h
+* @author LDRobot (marketing1@ldrobot.com)
+* @brief
+* @version 0.1
+* @date 2022.04.08
+* @note
+* @copyright Copyright (c) 2021 SHENZHEN LDROBOT CO., LTD. All rights reserved.
+**/
+
+#ifndef __LISTEN_NODE_H__
+#define __LISTEN_NODE_H__
+
+
+
+
+
+#endif //__LISTEN_NODE_H__
+/********************* (C) COPYRIGHT SHENZHEN LDROBOT CO., LTD *******END OF FILE ********/
diff --git a/rviz/ldlidar.rviz b/rviz/ldlidar.rviz
index 28e3663..b09cac2 100644
--- a/rviz/ldlidar.rviz
+++ b/rviz/ldlidar.rviz
@@ -68,9 +68,9 @@ Visualization Manager:
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
- Max Intensity: 249
+ Max Intensity: 246
Min Color: 0; 0; 0
- Min Intensity: 90
+ Min Intensity: 25
Name: LaserScan
Position Transformer: XYZ
Queue Size: 100
@@ -115,7 +115,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
- Distance: 6.14117813
+ Distance: 13.4815817
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
@@ -133,7 +133,7 @@ Visualization Manager:
Pitch: 1.56979632
Target Frame:
Value: Orbit (rviz)
- Yaw: 3.16539598
+ Yaw: 3.16039586
Saved: ~
Window Geometry:
Displays:
diff --git a/src/lipkg.cpp b/src/lipkg.cpp
index 8d010a5..f5a89a2 100644
--- a/src/lipkg.cpp
+++ b/src/lipkg.cpp
@@ -61,12 +61,17 @@ uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) {
return crc;
}
-LiPkg::LiPkg(std::string frame_id)
+LiPkg::LiPkg(std::string frame_id, bool laser_scan_dir, bool enable_angle_crop_func,
+ double angle_crop_min, double angle_crop_max)
: timestamp_(0),
speed_(0),
error_times_(0),
is_frame_ready_(false),
- is_pkg_ready_(false) {
+ is_pkg_ready_(false),
+ 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;
}
@@ -135,8 +140,6 @@ bool LiPkg::Parse(const uint8_t *data, long len) {
float step = diff / (POINT_PER_PACK - 1) / 100.0;
float start = (double)pkg.start_angle / 100.0;
float end = (double)(pkg.end_angle % 36000) / 100.0;
- // std::cout << "start " << start << std::endl;
- // std::cout << "end " << end << std::endl;
PointData data;
for (int i = 0; i < POINT_PER_PACK; i++) {
data.distance = pkg.point[i].distance;
@@ -146,12 +149,7 @@ bool LiPkg::Parse(const uint8_t *data, long len) {
}
data.intensity = pkg.point[i].intensity;
one_pkg_[i] = data;
- // std::cout << "data.angle " << data.angle << " data.distance " <<
- // data.distance
- // << " data.intensity " << (int)data.intensity <<
- // std::endl;
- frame_tmp_.push_back(
- PointData(data.angle, data.distance, data.intensity));
+ frame_tmp_.push_back(PointData(data.angle, data.distance, data.intensity));
}
// prevent angle invert
one_pkg_.back().angle = end;
@@ -239,7 +237,23 @@ void LiPkg::ToLaserscan(std::vector src) {
unsigned int last_index = 0;
for (auto point : src) {
float range = point.distance / 1000.f; // distance unit transform to meters
- float dir_angle = static_cast(360.f - point.angle); // Lidar rotation data flow changed from clockwise to counterclockwise
+ float intensity = point.intensity; // laser receive intensity
+ float dir_angle;
+
+ if (laser_scan_dir_) {
+ dir_angle = static_cast(360.f - point.angle); // Lidar rotation data flow changed
+ // from clockwise to counterclockwise
+ } else {
+ dir_angle = point.angle;
+ }
+
+ if (enable_angle_crop_func_) { // Angle crop setting, Mask data within the set angle range
+ if ((dir_angle >= angle_crop_min_) && (dir_angle <= angle_crop_max_)) {
+ range = 0;
+ intensity = 0;
+ }
+ }
+
float angle = ANGLE_TO_RADIAN(dir_angle); // Lidar angle unit form degree transform to radian
unsigned int index = (unsigned int)((angle - output_.angle_min) / output_.angle_increment);
if (index < beam_size) {
@@ -249,7 +263,7 @@ void LiPkg::ToLaserscan(std::vector src) {
unsigned int err = index - last_index;
if (err == 2){
output_.ranges[index - 1] = range;
- output_.intensities[index - 1] = point.intensity;
+ output_.intensities[index - 1] = intensity;
}
} else { // Otherwise, only when the distance is less than the current
// value, it can be re assigned
@@ -257,7 +271,7 @@ void LiPkg::ToLaserscan(std::vector src) {
output_.ranges[index] = range;
}
}
- output_.intensities[index] = point.intensity;
+ output_.intensities[index] = intensity;
last_index = index;
}
}
diff --git a/src/lipkg.h b/src/lipkg.h
index f36c67a..30dba87 100644
--- a/src/lipkg.h
+++ b/src/lipkg.h
@@ -55,7 +55,8 @@ typedef struct __attribute__((packed)) {
class LiPkg {
public:
- LiPkg(std::string frame_id);
+ LiPkg(std::string frame_id, bool laser_scan_dir, bool enable_angle_crop_func,
+ double angle_crop_min, double angle_crop_max);
// get Lidar spin speed (Hz)
double GetSpeed(void);
// get time stamp of the packet
@@ -84,12 +85,16 @@ 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 data_tmp_;
std::array one_pkg_;
std::vector frame_tmp_;
- bool is_pkg_ready_;
- bool is_frame_ready_;
sensor_msgs::LaserScan output_;
// Lidar frame data tranfrom to sensor_msg/LaserScan type data
void ToLaserscan(std::vector src);
diff --git a/src/main.cpp b/src/main.cpp
index 051b66c..8e9ed83 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -36,18 +36,28 @@ int main(int argc, char **argv) {
std::string topic_name;
std::string port_name;
std::string frame_id;
+ bool laser_scan_dir = true;
+ bool enable_angle_crop_func = false;
+ 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);
- ROS_INFO("[ldrobot] SDK Pack Version is v2.2.5");
+ ROS_INFO("[ldrobot] SDK Pack Version is v2.2.6");
ROS_INFO("[ldrobot] : %s,: %s,: %s,: %s",
product_name.c_str(), topic_name.c_str(), port_name.c_str(), frame_id.c_str());
+ ROS_INFO("[ldrobot] : %s,: %s,: %f,: %f",
+ (laser_scan_dir?"Counterclockwise":"Clockwise"), (enable_angle_crop_func?"true":"false"), angle_crop_min, angle_crop_max);
- LiPkg *lidar = new LiPkg(frame_id);
+ LiPkg *lidar = new LiPkg(frame_id, laser_scan_dir, enable_angle_crop_func, angle_crop_min, angle_crop_max);
CmdInterfaceLinux cmd_port;
if (port_name.empty()) {
@@ -78,21 +88,8 @@ int main(int argc, char **argv) {
if (lidar->IsFrameReady()) {
lidar_pub.publish(lidar->GetLaserScan()); // Fixed Frame: lidar_frame
lidar->ResetFrameReady();
-#if 0
- sensor_msgs::LaserScan data = lidar->GetLaserScan();
- unsigned int lens = (data.angle_max - data.angle_min) / data.angle_increment;
- std::cout << "[ldrobot] current_speed: " << lidar->GetSpeed() << " "
- << "len: " << lens << " "
- << "angle_min: " << RADIAN_TO_ANGLED(data.angle_min) << " "
- << "angle_max: " << RADIAN_TO_ANGLED(data.angle_max) << std::endl;
- std::cout << "----------------------------" << std::endl;
- for (unsigned int i = 0; i < lens; i++)
- {
- std::cout << "[ldrobot] range: " << data.ranges[i] << " "
- << "intensites: " << data.intensities[i] << std::endl;
- }
- std::cout << "----------------------------" << std::endl;
-#endif
+ ROS_INFO_STREAM("[ldrobot] current_speed(Hz): " << lidar->GetSpeed());
+ ROS_INFO_STREAM("----^---^-----");
}
r.sleep();
}
diff --git a/ws_deploy.sh b/ws_deploy.sh
index dd619e4..a724c32 100644
--- a/ws_deploy.sh
+++ b/ws_deploy.sh
@@ -3,8 +3,7 @@
#Date: 2022-02
floder_name=$1
null_name=" "
-if [ ${floder_name} == ${null_name} ]
-then
+if [ ${floder_name} -eq ${null_name} ];then
echo "please input \"./ws_deploy.sh floder_name\""
else
floder_name="${floder_name}_`date +%Y%m%d-%H-%M`"
@@ -16,6 +15,7 @@ else
cp ./launch ./${floder_name} -a
cp ./rviz ./${floder_name} -a
cp ./src ./${floder_name} -a
+ cp ./listen ./${floder_name} -a
zip -r ${floder_name}.zip ./${floder_name}
rm -rf ./${floder_name}/
echo "create ./${floder_name}.zip "