[add]1. 增加角度裁剪功能，可以在launch文件中配置不显示点云的角度区间;

2. 增加激光扫描方向可设置功能，可以在launch文件中配置;
3. 增加雷达消息的订阅节点;
4. 增加了.gitignore

.gitignore

@@ -0,0 +1,2 @@
+.vscode
+*.zip

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}

launch/ld06.launch

@@ -1,8 +1,26 @@
 <launch>
+<!-- ldldiar message publisher node -->
  <node name="LD06" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_node" output="screen" >
- <param name="product_name" value="LDLiDAR_LD06"/>
- <param name="topic_name" value="LiDAR/LD06"/>
- <param name="port_name" value ="/dev/ttyUSB0"/>
- <param name="frame_id" value="lidar_frame"/>
+  <param name="product_name" value="LDLiDAR_LD06"/>
+  <param name="topic_name" value="LiDAR/LD06"/>
+  <param name="port_name" value ="/dev/ttyUSB0"/>
+  <param name="frame_id" value="lidar_frame"/>
+  <!-- Set laser scan directon: -->
+  <!--    1. Set counterclockwise, example: <param name="laser_scan_dir" type="bool" value="true"/> -->
+  <!--    2. Set clockwise,        example: <param name="laser_scan_dir" type="bool" value="false"/> -->
+  <param name="laser_scan_dir" type="bool" value="true"/>
+  <!-- Angle crop setting, Mask data within the set angle range -->
+  <!--    1. Enable angle crop fuction: -->
+  <!--       1.1. enable angle crop,  example: <param name="enable_angle_crop_func" type="bool" value="true"/> -->
+  <!--       1.2. disable angle crop, example: <param name="enable_angle_crop_func" type="bool" value="false"/> -->
+  <param name="enable_angle_crop_func" type="bool" value="false"/>
+  <!--    2. Angle cropping interval setting, The distance and intensity data within the set angle range will be set to 0 --> 
+  <!--       angle >= "angle_crop_min" and angle <= "angle_crop_max", unit is degress -->
+  <param name="angle_crop_min" type="double" value="135.0"/>
+  <param name="angle_crop_max" type="double" value="225.0"/>
+ </node>
+<!-- ldlidar message subscriber node -->
+ <node name="ListenLD06" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
+  <param name="topic_name" value="LiDAR/LD06"/>
  </node>
 </launch>

launch/ld19.launch

@@ -1,8 +1,26 @@
 <launch>
+<!-- ldldiar message publisher node -->
  <node name="LD19" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_node" output="screen" >
- <param name="product_name" value="LDLiDAR_LD19"/>
- <param name="topic_name" value="LiDAR/LD19"/>
- <param name="port_name" value ="/dev/ttyUSB0"/>
- <param name="frame_id" value="lidar_frame"/>
+  <param name="product_name" value="LDLiDAR_LD19"/>
+  <param name="topic_name" value="LiDAR/LD19"/>
+  <param name="port_name" value ="/dev/ttyUSB0"/>
+  <param name="frame_id" value="lidar_frame"/>
+  <!-- Set laser scan directon: -->
+  <!--    1. Set counterclockwise, example: <param name="laser_scan_dir" type="bool" value="true"/> -->
+  <!--    2. Set clockwise,        example: <param name="laser_scan_dir" type="bool" value="false"/> -->
+  <param name="laser_scan_dir" type="bool" value="true"/>
+  <!-- Angle crop setting, Mask data within the set angle range -->
+  <!--    1. Enable angle crop fuction: -->
+  <!--       1.1. enable angle crop,  example: <param name="enable_angle_crop_func" type="bool" value="true"/> -->
+  <!--       1.2. disable angle crop, example: <param name="enable_angle_crop_func" type="bool" value="false"/> -->
+  <param name="enable_angle_crop_func" type="bool" value="false"/>
+  <!--    2. Angle cropping interval setting, The distance and intensity data within the set angle range will be set to 0 --> 
+  <!--       angle >= "angle_crop_min" and angle <= "angle_crop_max", unit is degress -->
+  <param name="angle_crop_min" type="double" value="135.0"/>
+  <param name="angle_crop_max" type="double" value="225.0"/>
+ </node>
+<!-- ldlidar message subscriber node -->
+ <node name="ListenLD19" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
+  <param name="topic_name" value="LiDAR/LD19"/>
  </node>
 </launch>

listen/listen_node.cpp

@@ -0,0 +1,71 @@
+/**
+* @file         listen_node.cpp
+* @author       LDRobot ([email protected])
+* @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 <ros/ros.h>
+#include <sensor_msgs/LaserScan.h>
+
+#include <string>
+#include <stdlib.h>
+
+#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 <topic_name> is null");
+    exit(EXIT_FAILURE);
+  } else {
+    ROS_INFO("[ldrobot] input param <topic_name> 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 ********/

listen/listen_node.h

@@ -0,0 +1,19 @@
+/**
+* @file         listen_node.h
+* @author       LDRobot ([email protected])
+* @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 ********/

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: <Fixed Frame>
       Value: Orbit (rviz)
-      Yaw: 3.16539598
+      Yaw: 3.16039586
     Saved: ~
 Window Geometry:
   Displays:

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<PointData> 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<float>(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<float>(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,15 +263,15 @@ void LiPkg::ToLaserscan(std::vector<PointData> 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
           if (range < output_.ranges[index]) {
             output_.ranges[index] = range;
           }
         }
-        output_.intensities[index] = point.intensity;
+        output_.intensities[index] = intensity;
         last_index = index;
       }
     }

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<uint8_t> data_tmp_;
   std::array<PointData, POINT_PER_PACK> one_pkg_;
   std::vector<PointData> 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<PointData> src);

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] <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());
 
+  ROS_INFO("[ldrobot] <laser_scan_dir>: %s,<enable_angle_crop_func>: %s,<angle_crop_min>: %f,<angle_crop_max>: %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();
   }

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 "