[update]版本号v2.3.5，1. 开发者的PR： Organize the package config and clarify …

…the license #1 ； 2. 修改ROS laser scan消息的angle_increment计算方式； 3. 更新README
ldrobotSensorTeam · Sep 19, 2022 · 6aece8b · 6aece8b
1 parent 98cc3ac
commit 6aece8b
Show file tree

Hide file tree

Showing 10 changed files with 102 additions and 57 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -195,13 +195,23 @@ target_link_libraries(${PROJECT_NAME}_listen_node  ${catkin_LIBRARIES})
 #   scripts/my_python_script
 #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 # )
+install(PROGRAMS
+  scripts/create_udev_rules.sh
+  scripts/delete_udev_rules.sh
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
 
 ## Mark executables and/or libraries for installation
 # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
 #   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 #   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 #   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 # )
+install(TARGETS ${PROJECT_NAME}_node ${PROJECT_NAME}_listen_node
+  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}/
@@ -216,6 +226,9 @@ target_link_libraries(${PROJECT_NAME}_listen_node  ${catkin_LIBRARIES})
 #   # myfile2
 #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 # )
+install(DIRECTORY launch rules rviz
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
 
 #############
 ## Testing ##

diff --git a/README.md b/README.md
@@ -31,13 +31,19 @@ sudo chmod 777 /dev/ttyUSB0
 - 第三步，修改`launch/`目录下雷达产品型号对应的lanuch文件中的`port_name`值，以ld06.launch为例，如下所示.
 
 ```xml
+<?xml version="1.0"?>
 <launch>
+<arg name="topic_name" default="scan"/>
+<arg name="frame_id" default="base_laser"/>
+<arg name="port_name" default="/dev/ttyUSB0"/>
+<arg name="fix_to_base_link" default="true" />
+
 <!-- 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="scan"/>
-  <param name="port_name" value ="/dev/ttyUSB0"/>
-  <param name="frame_id" value="base_laser"/>
+  <param name="topic_name" value="$(arg topic_name)"/>
+  <param name="frame_id" value="$(arg frame_id)"/>
+  <param name="port_name" value ="$(arg port_name)"/>
   <!-- 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"/> -->
@@ -53,12 +59,12 @@ sudo chmod 777 /dev/ttyUSB0
   <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">
+ <!-- node name="ListenLD06" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
   <param name="topic_name" value="scan"/>
- </node>
+ </node -->
  <!-- publisher tf transform, parents frame is base_link, child frame is base_laser -->
  <!-- args="x y z yaw pitch roll parents_frame_id child_frame_id period_in_ms"-->
- <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50"/>
+ <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50" if="$(arg fix_to_base_link)"/>
 </launch>
 ```
 ## 2. 编译方法
@@ -129,13 +135,6 @@ catkin_make
 ```bash
 rviz
 ```
-
-| 产品型号:          | Fixed Frame: | Topic:        |
-| ------------------ | ------------ | ------------- |
-| LDROBOT LiDAR LD06 | base_laser  | /scan |
-| LDROBOT LiDAR LD19 | base_laser  | /scan |
-
-
 # Instructions
 
 > This SDK is only applicable to the LiDAR products sold by Shenzhen LDROBOT Co., LTD. The product models are :
@@ -168,13 +167,19 @@ sudo chmod 777 /dev/ttyUSB0
 - Modify the `port_name` value in the Lanuch file corresponding to the radar product model under `launch/`, using `ld06.launch` as an example, as shown below.
 
 ``` xml
+<?xml version="1.0"?>
 <launch>
+<arg name="topic_name" default="scan"/>
+<arg name="frame_id" default="base_laser"/>
+<arg name="port_name" default="/dev/ttyUSB0"/>
+<arg name="fix_to_base_link" default="true" />
+
 <!-- 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="scan"/>
-  <param name="port_name" value ="/dev/ttyUSB0"/>
-  <param name="frame_id" value="base_laser"/>
+  <param name="topic_name" value="$(arg topic_name)"/>
+  <param name="frame_id" value="$(arg frame_id)"/>
+  <param name="port_name" value ="$(arg port_name)"/>
   <!-- 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"/> -->
@@ -190,12 +195,12 @@ sudo chmod 777 /dev/ttyUSB0
   <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">
+ <!-- node name="ListenLD06" pkg="ldlidar_stl_ros" type="ldlidar_stl_ros_listen_node" output="screen">
   <param name="topic_name" value="scan"/>
- </node>
+ </node -->
  <!-- publisher tf transform, parents frame is base_link, child frame is base_laser -->
  <!-- args="x y z yaw pitch roll parents_frame_id child_frame_id period_in_ms"-->
- <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50"/>
+ <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50" if="$(arg fix_to_base_link)"/>
 </launch>
 ```
 ## step 2: build
@@ -266,9 +271,4 @@ catkin_make
 - new a terminal (Ctrl + Alt + T) and use Rviz tool,open the rviz config file below the rviz folder of the readme file directory
 ```bash
 rviz
-```
-
-| Product:          | Fixed Frame: | Topic:        |
-| ------------------ | ------------ | ------------- |
-| LDROBOT LiDAR LD06 | base_laser  | /scan   |
-| LDROBOT LiDAR LD19 | base_laser  | /scan   |
+```
diff --git a/launch/ld06.launch b/launch/ld06.launch
@@ -1,10 +1,16 @@
+<?xml version="1.0"?>
 <launch>
+<arg name="topic_name" default="scan"/>
+<arg name="frame_id" default="base_laser"/>
+<arg name="port_name" default="/dev/ttyUSB0"/>
+<arg name="fix_to_base_link" default="true" />
+
 <!-- 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="scan"/>
-  <param name="port_name" value ="/dev/ttyUSB0"/>
-  <param name="frame_id" value="base_laser"/>
+  <param name="topic_name" value="$(arg topic_name)"/>
+  <param name="frame_id" value="$(arg frame_id)"/>
+  <param name="port_name" value ="$(arg port_name)"/>
   <!-- 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"/> -->
@@ -25,5 +31,5 @@
  </node -->
  <!-- publisher tf transform, parents frame is base_link, child frame is base_laser -->
  <!-- args="x y z yaw pitch roll parents_frame_id child_frame_id period_in_ms"-->
- <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50"/>
+ <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50" if="$(arg fix_to_base_link)"/>
 </launch>
diff --git a/launch/ld19.launch b/launch/ld19.launch
@@ -1,10 +1,16 @@
+<?xml version="1.0"?>
 <launch>
+<arg name="topic_name" default="scan"/>
+<arg name="frame_id" default="base_laser"/>
+<arg name="port_name" default="/dev/ttyUSB0"/>
+<arg name="fix_to_base_link" default="true" />
+
 <!-- 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="scan"/>
-  <param name="port_name" value ="/dev/ttyUSB0"/>
-  <param name="frame_id" value="base_laser"/>
+  <param name="topic_name" value="$(arg topic_name)"/>
+  <param name="frame_id" value="$(arg frame_id)"/>
+  <param name="port_name" value ="$(arg port_name)"/>
   <!-- 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"/> -->
@@ -25,5 +31,5 @@
  </node -->
  <!-- publisher tf transform, parents frame is base_link, child frame is base_laser -->
  <!-- args="x y z yaw pitch roll parents_frame_id child_frame_id period_in_ms"-->
- <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50"/>
+ <node name="base_to_laser" pkg="tf" type="static_transform_publisher"  args="0.0 0.0 0.18 0 0.0 0.0 base_link base_laser 50" if="$(arg fix_to_base_link)"/>
 </launch>
diff --git a/ldlidar_driver/lipkg.cpp b/ldlidar_driver/lipkg.cpp
@@ -57,7 +57,7 @@ uint8_t CalCRC8(const uint8_t *data, uint16_t data_len) {
 }
 
 LiPkg::LiPkg()
-    : sdk_pack_version_("v2.3.4"),
+    : sdk_pack_version_("v2.3.5"),
       timestamp_(0),
       speed_(0),
       error_times_(0),

diff --git a/package.xml b/package.xml
@@ -1,19 +1,19 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>ldlidar_stl_ros</name>
-  <version>0.0.0</version>
+  <version>2.3.5</version>
   <description>The ldlidar package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="support@ldrobot.com">marketing1</maintainer>
+  <maintainer email="support@ldrobot.com">SHENZHEN LDROBOT CO., LTD.</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->
   <!-- Commonly used license strings: -->
   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
+  <license>MIT</license>
 
 
   <!-- Url tags are optional, but multiple are allowed, one per tag -->
@@ -48,25 +48,37 @@
   <!--   <test_depend>gtest</test_depend> -->
   <!-- Use doc_depend for packages you need only for building documentation: -->
   <!--   <doc_depend>doxygen</doc_depend> -->
+
   <buildtool_depend>catkin</buildtool_depend>
+
   <!-- <build_depend>pcl_conversions</build_depend> -->
   <!-- <build_depend>pcl_ros</build_depend> -->
-  <build_depend>roscpp</build_depend>
-  <build_depend>sensor_msgs</build_depend>
+
+  <!-- build_depend>roscpp</build_depend -->
+  <!-- build_depend>sensor_msgs</build_depend -->
+
   <!-- <build_export_depend>pcl_conversions</build_export_depend> -->
   <!-- <build_export_depend>pcl_ros</build_export_depend> -->
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>sensor_msgs</build_export_depend>
+
+  <!-- build_export_depend>roscpp</build_export_depend -->
+  <!-- build_export_depend>sensor_msgs</build_export_depend -->
+
   <!-- <exec_depend>pcl_conversions</exec_depend> -->
   <!-- <exec_depend>pcl_ros</exec_depend> -->
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
+
+  <!-- exec_depend>roscpp</exec_depend -->
+  <!-- exec_depend>sensor_msgs</exec_depend -->
+
   <!-- <build_depend>libpcl-all-dev</build_depend> -->
   <!-- <exec_depend>libpcl-all</exec_depend> -->
 
+  <depend>roscpp</depend>
+  <depend>sensor_msgs</depend>
+  <exec_depend>tf</exec_depend>
+
   <!-- The export tag contains other, unspecified, tags -->
-  <export>
+  <!-- export -->
     <!-- Other tools can request additional information be placed here -->
 
-  </export>
+  <!-- /export -->
 </package>
diff --git a/scripts/ldlidar.rules → rules/ldlidar.rules b/scripts/ldlidar.rules → rules/ldlidar.rules
diff --git a/scripts/create_udev_rules.sh b/scripts/create_udev_rules.sh
@@ -3,7 +3,7 @@
 echo "remap the device serial port(ttyUSBX) to  ldlidar"
 echo "ldlidar usb connection as /dev/ldlidar , check it using the command : ls -l /dev|grep ttyUSB"
 echo "start copy ldlidar.rules to  /etc/udev/rules.d/"
-sudo cp ./ldlidar.rules  /etc/udev/rules.d
+sudo cp `rospack find ldlidar_stl_ros`/rules/ldlidar.rules /etc/udev/rules.d/
 echo " "
 echo "Restarting udev"
 echo ""

diff --git a/scripts/delete_udev_rules.sh b/scripts/delete_udev_rules.sh
@@ -2,7 +2,7 @@
 
 echo "delete remap the device serial port(ttyUSBX) to  ldlidar"
 echo "sudo rm   /etc/udev/rules.d/ldlidar.rules"
-sudo rm   /etc/udev/rules.d/ldlidar.rules
+sudo rm /etc/udev/rules.d/ldlidar.rules
 echo " "
 echo "Restarting udev"
 echo ""

diff --git a/src/publish_node/main.cpp b/src/publish_node/main.cpp
@@ -21,7 +21,8 @@
 #include "ros_api.h"
 #include "lipkg.h"
 
-void  ToLaserscanMessagePublish(ldlidar::Points2D& src, ldlidar::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");
@@ -104,27 +105,33 @@ int main(int argc, char **argv) {
   return 0;
 }
 
-void  ToLaserscanMessagePublish(ldlidar::Points2D& src, ldlidar::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;
   static ros::Time end_scan_time;
+  static bool first_scan = true;
 
   start_scan_time = ros::Time::now();
   scan_time = (start_scan_time - end_scan_time).toSec();
 
+  if (first_scan) {
+    first_scan = false;
+    end_scan_time = start_scan_time;
+    return;
+  }
+
   // Adjust the parameters according to the demand
-  angle_min = ANGLE_TO_RADIAN(src.front().angle);
-  angle_max = ANGLE_TO_RADIAN(src.back().angle);
+  angle_min = 0;
+  angle_max = (2 * M_PI);
   range_min = 0.02;
   range_max = 12;
-  float spin_speed = static_cast<float>(commpkg->GetSpeedOrigin());
-  float scan_freq = static_cast<float>(commpkg->kPointFrequence);
-  angle_increment = ANGLE_TO_RADIAN(spin_speed/scan_freq);
+  int beam_size = static_cast<int>(src.size());
+  angle_increment = (angle_max - angle_min) / (float)(beam_size -1);
 
   // Calculate the number of scanning points
   if (commpkg->GetSpeedOrigin() > 0) {
-    int beam_size = static_cast<int>(ceil((angle_max - angle_min) / angle_increment));
     sensor_msgs::LaserScan output;
     output.header.stamp = start_scan_time;
     output.header.frame_id = setting.frame_id;
@@ -161,10 +168,11 @@ void  ToLaserscanMessagePublish(ldlidar::Points2D& src, ldlidar::LiPkg* commpkg,
       }
 
       float angle = ANGLE_TO_RADIAN(dir_angle); // Lidar angle unit form degree transform to radian
-      int index = (int)((angle - output.angle_min) / output.angle_increment);
+      int index = static_cast<int>(ceil((angle - angle_min) / angle_increment));
       if (index < beam_size) {
         if (index < 0) {
-          ROS_ERROR("[ldrobot] error index: %d, beam_size: %d, angle: %f, output.angle_min: %f, output.angle_increment: %f", index, beam_size, angle, output.angle_min, output.angle_increment);
+          ROS_ERROR("[ldrobot] error index: %d, beam_size: %d, angle: %f, angle_min: %f, angle_increment: %f", 
+              index, beam_size, angle, angle_min, angle_increment);
         }
 
         if (setting.laser_scan_dir) {