point_lio

src/FAST_LIO/CMakeLists.txt

@@ -66,6 +66,7 @@ include_directories(
   ${EIGEN3_INCLUDE_DIR}
   ${PCL_INCLUDE_DIRS}
   ${PYTHON_INCLUDE_DIRS}
+  ${CMAKE_BINARY_DIR}/devel/include
   include)
 
 add_message_files(
@@ -75,7 +76,7 @@ add_message_files(
 
 generate_messages(
  DEPENDENCIES
- geometry_msgs
+ std_msgs geometry_msgs
 )
 
 catkin_package(

src/FAST_LIO/config/avia.yaml

@@ -1,7 +1,7 @@
 common:
     lid_topic:  "/livox/lidar"
     imu_topic:  "/livox/imu"
-    time_sync_en: false         # ONLY turn on when external time synchronization is really not possible
+    time_sync_en: true         # ONLY turn on when external time synchronization is really not possible
     time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other algorithms, e.g. LI-Init (can be found in README).
                                   # This param will take effect no matter what time_sync_en is. So if the time offset is not known exactly, please set as 0.0
 

src/FAST_LIO/config/mid360.yaml

@@ -17,7 +17,7 @@ mapping:
     b_gyr_cov: 0.0001
     fov_degree:    360
     det_range:     100.0
-    extrinsic_est_en:  false      # true: enable the online estimation of IMU-LiDAR extrinsic
+    extrinsic_est_en:  False      # true: enable the online estimation of IMU-LiDAR extrinsic
     extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
     extrinsic_R: [ 1, 0, 0,
                    0, 1, 0,

src/FAST_LIO/launch/mapping_mid360.launch

@@ -7,13 +7,13 @@
 
 	<param name="feature_extract_enable" type="bool" value="0"/>
 	<param name="point_filter_num" type="int" value="3"/>
-	<param name="max_iteration" type="int" value="3" />
+	<param name="max_iteration" type="int" value="6" />
 	<param name="filter_size_surf" type="double" value="0.5" />
 	<param name="filter_size_map" type="double" value="0.5" />
 	<param name="cube_side_length" type="double" value="1000" />
-	<param name="runtime_pos_log_enable" type="bool" value="0" />
+	<param name="runtime_pos_log_enable" type="bool" value="1" />
     <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
-
+	<node pkg="tf" type="static_transform_publisher" name="livox_tf" args="0 0 0 180 0 0 body camera_init 100"/>
 	<group if="$(arg rviz)">
 	<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
 	</group>

src/FAST_LIO/src/laserMapping.cpp

@@ -488,6 +488,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
         {
             RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
                                 &laserCloudWorld->points[i]);
+
         }
 
         sensor_msgs::PointCloud2 laserCloudmsg;
@@ -538,6 +539,7 @@ void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
     {
         RGBpointBodyLidarToIMU(&feats_undistort->points[i], \
                             &laserCloudIMUBody->points[i]);
+
     }
 
     sensor_msgs::PointCloud2 laserCloudmsg;
@@ -764,8 +766,8 @@ int main(int argc, char** argv)
     nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true);
     nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);
     nh.param<string>("map_file_path",map_file_path,"");
-    nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");
+    nh.param<string>("common/lid_topic",lid_topic,"livox/lidar");
-    nh.param<string>("common/imu_topic", imu_topic,"/livox/imu");
+    nh.param<string>("common/imu_topic", imu_topic,"livox/imu");
     nh.param<bool>("common/time_sync_en", time_sync_en, false);
     nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0);
     nh.param<double>("filter_size_corner",filter_size_corner_min,0.5);
@@ -847,17 +849,17 @@ int main(int argc, char** argv)
         nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
     ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
     ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
-            ("/cloud_registered", 100000);
+            ("cloud_registered", 100000);
     ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
-            ("/cloud_registered_body", 100000);
+            ("cloud_registered_body", 100000);
     ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
-            ("/cloud_effected", 100000);
+            ("cloud_effected", 100000);
     ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
-            ("/Laser_map", 100000);
+            ("Laser_map", 100000);
     ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> 
-            ("/Odometry", 100000);
+            ("Odometry", 100000);
     ros::Publisher pubPath          = nh.advertise<nav_msgs::Path> 
-            ("/path", 100000);
+            ("path", 100000);
 //------------------------------------------------------------------------------------------------------
     signal(SIGINT, SigHandle);
     ros::Rate rate(5000);

src/FAST_LIO_LOCALIZATION/CMakeLists.txt

@@ -55,7 +55,9 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   eigen_conversions
   fast_lio
+  serial
   genmsg
+  Eigen3
 )
 
 find_package(Eigen3 REQUIRED)
@@ -72,12 +74,23 @@ include_directories(
 
 add_message_files(
   FILES
+  underpan.msg
   Pose6D.msg
+  serial.msg
+  DataMCU.msg
+  DataRef.msg
+  DataAI.msg
+  Ps2Data.msg
+  GoalPose.msg
+  DataNav.msg
+  position.msg
+  
 )
 
 generate_messages(
  DEPENDENCIES
  geometry_msgs
+ std_msgs
 )
 
 catkin_package(
@@ -86,13 +99,42 @@ catkin_package(
   INCLUDE_DIRS
 )
 
-add_executable(fastlio_mapping1 src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
+# add_executable(fastlio_mapping1 src/laserMapping.cpp include/ikd-Tree/ikd_Tree.cpp src/preprocess.cpp)
 add_executable(flag src/flag.cpp)
-add_executable(controlpid src/controlpid.cpp src/mbot_linux_serial.cpp)
+add_executable(uart src/uart.cpp)
-target_link_libraries(fastlio_mapping1 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+add_executable(send src/send.cpp)
+add_executable(test_goal src/test_goal.cpp)
+add_executable(underpan_serial src/underpan_serial.cpp)
+add_executable(tor1_serial src/tor1_serial.cpp)
+add_executable(position src/position.cpp)
+add_executable(all_move src/all_move.cpp)
+add_executable(chong src/chong.cpp src/mbot_linux_serial.cpp)
+add_executable(chong0 src/chong0.cpp src/mbot_linux_serial.cpp)
+add_executable(commands src/commands.cpp )
+# target_link_libraries(fastlio_mapping1 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
 target_link_libraries(flag ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
-target_link_libraries(controlpid ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(uart ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
-target_include_directories(fastlio_mapping1 PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_link_libraries(test_goal ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(send ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(underpan_serial ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(tor1_serial ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(position ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(all_move ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(chong ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(chong0 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+target_link_libraries(commands ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES})
+# target_include_directories(fastlio_mapping1 PRIVATE ${PYTHON_INCLUDE_DIRS})
 target_include_directories(flag PRIVATE ${PYTHON_INCLUDE_DIRS})
-target_include_directories(controlpid PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(uart PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(test_goal PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(send PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(underpan_serial PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(tor1_serial PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(position PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(all_move PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(chong PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(chong0 PRIVATE ${PYTHON_INCLUDE_DIRS})
+target_include_directories(commands PRIVATE ${PYTHON_INCLUDE_DIRS})
 add_dependencies(fastlio_mapping fast_lio_generate_messages_cpp)
+add_dependencies(underpan_serial ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(commands ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

src/FAST_LIO_LOCALIZATION/PCD/map.yaml

@@ -0,0 +1,7 @@
+image: map.pgm
+resolution: 0.050000
+origin: [-12.310993, -12.513004, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/FAST_LIO_LOCALIZATION/PCD/map1.yaml

@@ -1,6 +1,6 @@
-image: /home/cmrt/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/map1.pgm
+image: map1.pgm
 resolution: 0.050000
-origin: [-13.175220, -3.068274, 0.000000]
+origin: [-17.402567, -15.452375, 0.000000]
 negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196

src/FAST_LIO_LOCALIZATION/PCD/map_test.yaml

@@ -0,0 +1,7 @@
+image: map_test.pgm
+resolution: 0.050000
+origin: [-45.092827, -36.606453, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/FAST_LIO_LOCALIZATION/PCD/mapc.yaml

@@ -0,0 +1,7 @@
+image: mapc.pgm
+resolution: 0.050000
+origin: [-63.628784, -48.638741, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch

@@ -5,8 +5,8 @@
         
         <rosparam>
             transform_tolerance: 0.01
-            min_height: 0.0
+            min_height: 1
-            max_height: 1.0
+            max_height: 5.0
 
             angle_min: -3.14159 # -M_PI/2
             angle_max: 3.14159 # M_PI/2

src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch

@@ -1,22 +1,21 @@
 <launch>
 <!-- Launch file for Livox AVIA LiDAR -->
-
 	<arg name="rviz" default="true" />
 
 	<rosparam command="load" file="$(find fast_lio_localization)/config/mid360.yaml" />
 
 	<param name="feature_extract_enable" type="bool" value="1"/>
-	<param name="point_filter_num" type="int" value="3"/>
+	<param name="point_filter_num" type="int" value="2"/>
-	<param name="max_iteration" type="int" value="3" />
+	<param name="max_iteration" type="int" value="6" />
-	<param name="filter_size_surf" type="double" value="0.5" />
+	<param name="filter_size_surf" type="double" value="0.3" />
-	<param name="filter_size_map" type="double" value="0.5" />
+	<param name="filter_size_map" type="double" value="0.3" />
 	<param name="cube_side_length" type="double" value="1000" />
 	<param name="runtime_pos_log_enable" type="bool" value="0" />
-    <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" /> 
+    <!-- <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping"   />  -->
-
+ <!--  -->
 
 	<!-- <arg name="map" default="/home/rm/ws_sentry/src/FAST_LIO/PCD(305_10-20)/scans.pcd" /> -->
-	<arg name="map" default="$(find fast_lio_localization)/PCD/scans.pcd" />
+	<arg name="map" default="$(find fast_lio)/PCD/scans1.pcd" />
 	<!-- loalization-->
     <node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
 
@@ -38,11 +37,9 @@
 	<!-- load 2d map -->
 	<arg name="2dmap" default="M4.yaml" />
 
-	<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/map1.yaml /map:=prior_map"/>
+	<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/map1.yaml /map:=prior_map" />
 
 	<!-- pointscloud2 to laserscans -->
 	<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">
 	</include>
-
-
 </launch>

src/FAST_LIO_LOCALIZATION/launch/move.launch

@@ -1,5 +0,0 @@
-<launch>
-<node pkg = "fast_lio_localization" type = "flag" name = "flag" output="screen"/>
-<node pkg = "fast_lio_localization" type = "controlpid" name = "controlpid" output="screen"/>
-
-</launch>

src/FAST_LIO_LOCALIZATION/launch/nav.launch

@@ -1,5 +1,7 @@
 <launch>
-<include file="$(find livox_ros_driver2)/launch_ROS1/msg_MID360.launch"/>
+<node pkg="fast_lio_localization" type="flag" name="flag" />
-<include file="$(find fast_lio_localization)/launch/localization_MID360.launch"/>
+<node pkg="fast_lio_localization" type="uart" name="uart" output="screen"/>
-
+<node pkg="fast_lio_localization" type="all_move" name="all_move" output="screen"/>
+<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="map_to_camera_init" 
+          args="0 0 0 0 0 0 map camera_init" /> -->
 </launch>

src/FAST_LIO_LOCALIZATION/launch/r_serial.launch

@@ -0,0 +1,5 @@
+<launch>
+<node pkg="fast_lio_localization" type="underpan_serial" name="underpan_serial" output="screen"/>
+<!-- <node pkg="fast_lio_localization" type="tor1_serial" name="tor1_serial" output="screen"/>
+<node pkg="fast_lio_localization" type="position" name="position" output="screen"/> -->
+</launch>

src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch

@@ -3,7 +3,7 @@
 
 	<arg name="rviz" default="true" />
 	<arg name="use_sim_time" value="true"/>
-	<arg name="map" default="$(find fast_lio)/PCD/scanss.pcd" />
+	<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
 
 	<!-- fast_lio -->
 	<rosparam command="load" file="$(find fast_lio)/config/mid360.yaml" />

src/FAST_LIO_LOCALIZATION/msg/DataAI.msg

@@ -0,0 +1,11 @@
+float32 yaw
+float32 pit
+float32 rol
+float32 vx
+float32 vy
+float32 wz
+float32 pos
+float32 ang
+bool reach
+bool achieve
+uint8 notice

src/FAST_LIO_LOCALIZATION/msg/DataMCU.msg

@@ -0,0 +1,8 @@
+float32 q0
+float32 q1
+float32 q2
+float32 q3
+float32 yaw
+float32 pit
+float32 rol
+uint8 notice

src/FAST_LIO_LOCALIZATION/msg/DataNav.msg

@@ -0,0 +1,5 @@
+bool reached
+
+float32 x
+float32 y
+float32 yaw

src/FAST_LIO_LOCALIZATION/msg/DataRef.msg

@@ -0,0 +1,3 @@
+uint16 remain_hp
+uint8 game_progress
+uint16 stage_remain_time

src/FAST_LIO_LOCALIZATION/msg/GoalPose.msg

@@ -0,0 +1,8 @@
+float32 x
+float32 y
+float32 angle
+
+float32 max_speed
+float32 tolerance
+float32 circle
+bool rotor

src/FAST_LIO_LOCALIZATION/msg/Ps2Data.msg

@@ -0,0 +1,20 @@
+# control input message
+float32 lx
+float32 ly
+float32 rx
+float32 ry
+
+float32 up_down
+float32 left_right
+
+bool l1
+bool l2
+bool r1
+bool r2
+
+# 四种模式
+uint8 mode # 0:手柄控制 1:键盘控制 2:自瞄 3:手动瞄准
+
+bool select
+bool start
+

src/FAST_LIO_LOCALIZATION/msg/position.msg

@@ -0,0 +1,3 @@
+int8 passball
+float32 x
+float32 y

src/FAST_LIO_LOCALIZATION/msg/serial.msg

@@ -0,0 +1 @@
+uint8[] data  # 定义二进制数据字段

src/FAST_LIO_LOCALIZATION/msg/underpan.msg

@@ -0,0 +1,2 @@
+int8 dian
+int8 passball

src/FAST_LIO_LOCALIZATION/package.xml

@@ -26,6 +26,7 @@
   <build_depend>tf</build_depend>
   <build_depend>pcl_ros</build_depend>
   <build_depend>livox_ros_driver2</build_depend>
+  <build_depend>serial</build_depend>
   <build_depend>message_generation</build_depend>
 
   <run_depend>geometry_msgs</run_depend>
@@ -37,6 +38,7 @@
   <run_depend>tf</run_depend>
   <run_depend>pcl_ros</run_depend>
   <run_depend>livox_ros_driver2</run_depend>
+  <run_depend>serial</run_depend>
   <run_depend>message_runtime</run_depend>
 
   <test_depend>rostest</test_depend>

src/FAST_LIO_LOCALIZATION/rviz_cfg/localization.rviz

@@ -17,7 +17,7 @@ Panels:
         - /Path1
         - /MarkerArray1/Namespaces1
       Splitter Ratio: 0.6382352709770203
-    Tree Height: 417
+    Tree Height: 897
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -444,17 +444,19 @@ Visualization Manager:
       Frame Timeout: 15
       Frames:
         All Enabled: true
+        bash:
+          Value: true
         body:
           Value: true
-        body_2d:
-          Value: true
-        body_foot:
-          Value: true
         camera_init:
           Value: true
+        car:
+          Value: true
+        goal_pose:
+          Value: true
         map:
           Value: true
-        robot_foot_init:
+        odom:
           Value: true
       Marker Alpha: 1
       Marker Scale: 1
@@ -464,13 +466,14 @@ Visualization Manager:
       Show Names: true
       Tree:
         map:
-          body_2d:
+          bash:
+            goal_pose:
               {}
           camera_init:
             body:
-              body_foot:
+              car:
                 {}
-            robot_foot_init:
+          odom:
             {}
       Update Interval: 0
       Value: true
@@ -549,10 +552,10 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 656
+  Height: 1136
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000001defc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000001de000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000001defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001de000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b800000052fc0100000002fb0000000800540069006d00650100000000000004b8000003bc00fffffffb0000000800540069006d006501000000000000045000000000000000000000035c000001de00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003befc020000000dfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003be000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650000000297000001dc0000000000000000fb0000000a0049006d0061006700650000000394000001600000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c70000000000000000fb0000000a0049006d00610067006501000002c5000000c700000000000000000000000100000152000001defc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000001de000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073800000052fc0100000002fb0000000800540069006d0065010000000000000738000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc000003be00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -561,6 +564,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1208
+  Width: 1848
   X: 72
   Y: 27

src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_build_map.rviz

@@ -18,7 +18,6 @@ Panels:
         - /Localization1/localization1
         - /Localization1/localization1/Shape1
         - /MarkerArray1/Namespaces1
-        - /Axes3/Status1
       Splitter Ratio: 0.6382352709770203
     Tree Height: 467
   - Class: rviz/Selection
@@ -481,7 +480,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 27.20014190673828
+      Distance: 36.58704376220703
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -497,9 +496,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 1.279795527458191
+      Pitch: 0.8997958898544312
       Target Frame: body
-      Yaw: 0.9917507767677307
+      Yaw: 6.254936218261719
     Saved: ~
 Window Geometry:
   Displays:
@@ -518,4 +517,4 @@ Window Geometry:
     collapsed: true
   Width: 1102
   X: 191
-  Y: 33
+  Y: 27

src/FAST_LIO_LOCALIZATION/scripts/global_localization.py

@@ -2,22 +2,26 @@
 # coding=utf8
 from __future__ import print_function, division, absolute_import
 
+import numpy as np
 import copy
 import _thread
 import time
 
 import open3d as o3d
 import rospy
-import ros_numpy
 from geometry_msgs.msg import PoseWithCovarianceStamped, Pose, Point, Quaternion
 from nav_msgs.msg import Odometry
+from std_msgs.msg import Bool
 from sensor_msgs.msg import PointCloud2
-import numpy as np
+from fast_lio_localization.msg import underpan
 import tf
 import tf.transformations
+from sensor_msgs.point_cloud2 import read_points, create_cloud_xyz32
 
 global_map = None
-initialized = False
+initialized = True
+dian = False
+th_ok_msg = False
 T_map_to_odom = np.eye(4)
 cur_odom = None
 cur_scan = None
@@ -31,18 +35,16 @@ def pose_to_mat(pose_msg):
 
 
 def msg_to_array(pc_msg):
-    pc_array = ros_numpy.numpify(pc_msg)
+    points_list = []
-    pc = np.zeros([len(pc_array), 3])
+    for point in read_points(pc_msg, field_names=("x", "y", "z"), skip_nans=True):
-    pc[:, 0] = pc_array['x']
+        points_list.append(point)
-    pc[:, 1] = pc_array['y']
+    return np.array(points_list)
-    pc[:, 2] = pc_array['z']
-    return pc
 
 
 def registration_at_scale(pc_scan, pc_map, initial, scale):
     result_icp = o3d.pipelines.registration.registration_icp(
         voxel_down_sample(pc_scan, SCAN_VOXEL_SIZE * scale), voxel_down_sample(pc_map, MAP_VOXEL_SIZE * scale),
-        1.0 * scale, initial,
+        3.0 * scale, initial,
         o3d.pipelines.registration.TransformationEstimationPointToPoint(),
         o3d.pipelines.registration.ICPConvergenceCriteria(max_iteration=20)
     )
@@ -60,19 +62,8 @@ def inverse_se3(trans):
 
 
 def publish_point_cloud(publisher, header, pc):
-    data = np.zeros(len(pc), dtype=[
+    # 创建 PointCloud2 消息
-        ('x', np.float32),
+    msg = create_cloud_xyz32(header, pc)
-        ('y', np.float32),
-        ('z', np.float32),
-        ('intensity', np.float32),
-    ])
-    data['x'] = pc[:, 0]
-    data['y'] = pc[:, 1]
-    data['z'] = pc[:, 2]
-    if pc.shape[1] == 4:
-        data['intensity'] = pc[:, 3]
-    msg = ros_numpy.msgify(PointCloud2, data)
-    msg.header = header
     publisher.publish(msg)
 
 
@@ -107,7 +98,7 @@ def crop_global_map_in_FOV(global_map, pose_estimation, cur_odom):
 
     # 发布fov内点云
     header = cur_odom.header
-    header.frame_id = 'map'
+    header.frame_id = 'camera_init'
     publish_point_cloud(pub_submap, header, np.array(global_map_in_FOV.points)[::10])
 
     return global_map_in_FOV
@@ -123,15 +114,30 @@ def global_localization(pose_estimation):
     scan_tobe_mapped = copy.copy(cur_scan)
 
     tic = time.time()
-
+    if cur_odom is None:
+        cur_odom = Odometry()
+        cur_odom.header.stamp = rospy.Time.now()
+        cur_odom.header.frame_id = "map"
+        cur_odom.pose.pose.position.x = 0.0
+        cur_odom.pose.pose.position.y = 0.0
+        cur_odom.pose.pose.position.z = 0.0
+        cur_odom.pose.pose.orientation.x = 0.0
+        cur_odom.pose.pose.orientation.y = 0.0
+        cur_odom.pose.pose.orientation.z = 0.0
+        cur_odom.pose.pose.orientation.w = 0.0
+        global_map_in_FOV = crop_global_map_in_FOV(global_map, pose_estimation, cur_odom)
+        rospy.logwarn("cur_odom is None. Using default pose for global localization.")
+    else:
         global_map_in_FOV = crop_global_map_in_FOV(global_map, pose_estimation, cur_odom)
 
+    # global_map_in_FOV = crop_global_map_in_FOV(global_map, pose_estimation, cur_odom)
+
     # 粗配准
-    transformation, _ = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=5)
+    transformation, _ = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=pose_estimation, scale=3.0)
 
     # 精配准
     transformation, fitness = registration_at_scale(scan_tobe_mapped, global_map_in_FOV, initial=transformation,
-                                                    scale=1)
+                                                    scale=0.6)
     toc = time.time()
     rospy.loginfo('Time: {}'.format(toc - tic))
     rospy.loginfo('')
@@ -149,11 +155,16 @@ def global_localization(pose_estimation):
         map_to_odom.header.stamp = cur_odom.header.stamp
         map_to_odom.header.frame_id = 'map'
         pub_map_to_odom.publish(map_to_odom)
+        th_ok_msg.data = True
+        th_ok.publish(th_ok_msg)
+
         return True
     else:
         rospy.logwarn('Not match!!!!')
         rospy.logwarn('{}'.format(transformation))
         rospy.logwarn('fitness score:{}'.format(fitness))
+        th_ok_msg.data = False
+        th_ok.publish(th_ok_msg)
         return False
 
 
@@ -188,23 +199,31 @@ def cb_save_cur_scan(pc_msg):
     pub_pc_in_map.publish(pc_msg)
 
     # 转换为pcd
-    # fastlio给的field有问题 处理一下
-    pc_msg.fields = [pc_msg.fields[0], pc_msg.fields[1], pc_msg.fields[2],
-                     pc_msg.fields[4], pc_msg.fields[5], pc_msg.fields[6],
-                     pc_msg.fields[3], pc_msg.fields[7]]
     pc = msg_to_array(pc_msg)
 
     cur_scan = o3d.geometry.PointCloud()
     cur_scan.points = o3d.utility.Vector3dVector(pc[:, :3])
 
 
+def dounderpan(msg):
+    global dian
+    dian = (msg.dian == 1)
+
+    # 处理underpan消息
+
+
 def thread_localization():
     global T_map_to_odom
     while True:
         # 每隔一段时间进行全局定位
-        rospy.sleep(1 / FREQ_LOCALIZATION)
         # TODO 由于这里Fast lio发布的scan是已经转换到odom系下了 所以每次全局定位的初始解就是上一次的map2odom 不需要再拿odom了
+        if dian:
             global_localization(T_map_to_odom)
+            rospy.loginfo('Global localization success')
+            rospy.sleep(1 / FREQ_LOCALIZATION)
+        else:
+
+            continue
 
 
 if __name__ == '__main__':
@@ -212,17 +231,17 @@ if __name__ == '__main__':
     SCAN_VOXEL_SIZE = 0.1
 
     # Global localization frequency (HZ)
-    FREQ_LOCALIZATION = 0.5
+    FREQ_LOCALIZATION = 10
 
     # The threshold of global localization,
     # only those scan2map-matching with higher fitness than LOCALIZATION_TH will be taken
-    LOCALIZATION_TH = 0.8
+    LOCALIZATION_TH = 0.6
 
     # FOV(rad), modify this according to your LiDAR type
     FOV = 6.28
 
     # The farthest distance(meters) within FOV
-    FOV_FAR = 30
+    FOV_FAR = 40
 
     rospy.init_node('fast_lio_localization')
     rospy.loginfo('Localization Node Inited...')
@@ -231,14 +250,16 @@ if __name__ == '__main__':
     pub_pc_in_map = rospy.Publisher('/cur_scan_in_map', PointCloud2, queue_size=1)
     pub_submap = rospy.Publisher('/submap', PointCloud2, queue_size=1)
     pub_map_to_odom = rospy.Publisher('/map_to_odom', Odometry, queue_size=1)
+    th_ok = rospy.Publisher('/global_localization_th_ok', Bool, queue_size=1)
 
     rospy.Subscriber('/cloud_registered', PointCloud2, cb_save_cur_scan, queue_size=1)
     rospy.Subscriber('/Odometry', Odometry, cb_save_cur_odom, queue_size=1)
+    rospy.Subscriber('/underpan', underpan, dounderpan, queue_size=1)
 
     # 初始化全局地图
     rospy.logwarn('Waiting for global map......')
     initialize_global_map(rospy.wait_for_message('/map', PointCloud2))
-
+    th_ok_msg = Bool()
     # 初始化
     while not initialized:
         rospy.logwarn('Waiting for initial pose....')
@@ -255,6 +276,12 @@ if __name__ == '__main__':
     rospy.loginfo('Initialize successfully!!!!!!')
     rospy.loginfo('')
     # 开始定期全局定位
+    # pose_msg = rospy.wait_for_message('/initialpose', PoseWithCovarianceStamped)
+    # initial_pose = pose_to_mat(pose_msg)
+    # th_ok_msg.data = global_localization(initial_pose)
+    # th_ok.publish(th_ok_msg)
+
+    
     _thread.start_new_thread(thread_localization, ())
 
     rospy.spin()

src/FAST_LIO_LOCALIZATION/scripts/publish_initial_pose.py

@@ -1,4 +1,4 @@
-#!/usr/bin/python3
+#!/usr/bin/python3cmd_pid_publishe
 # coding=utf8
 from __future__ import print_function, division, absolute_import
 

src/FAST_LIO_LOCALIZATION/src/.vscode/c_cpp_properties.json

@@ -0,0 +1,23 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/home/r1/1/devel/include/**",
+        "/opt/ros/noetic/include/**",
+        "/home/r1/1/src/FAST_LIO/include/**",
+        "/home/r1/1/src/FAST_LIO_LOCALIZATION/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-x64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}

src/FAST_LIO_LOCALIZATION/src/.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/home/r1/1/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/home/r1/1/devel/lib/python3/dist-packages",
+        "/opt/ros/noetic/lib/python3/dist-packages"
+    ]
+}

src/FAST_LIO_LOCALIZATION/src/IMU_Processing.cpp

@@ -0,0 +1,136 @@
+#include "IMU_Processing.h"
+
+const bool time_list(PointType &x, PointType &y) {return (x.curvature < y.curvature);};
+
+void ImuProcess::set_gyr_cov(const V3D &scaler)
+{
+  cov_gyr_scale = scaler;
+}
+
+void ImuProcess::set_acc_cov(const V3D &scaler)
+{
+  cov_vel_scale = scaler;
+}
+
+ImuProcess::ImuProcess()
+    : b_first_frame_(true), imu_need_init_(true)
+{
+  imu_en = true;
+  init_iter_num = 1;
+  mean_acc      = V3D(0, 0, 0.0);
+  mean_gyr      = V3D(0, 0, 0);
+  after_imu_init_ = false;
+  state_cov.setIdentity();
+}
+
+ImuProcess::~ImuProcess() {}
+
+void ImuProcess::Reset() 
+{
+  ROS_WARN("Reset ImuProcess");
+  mean_acc      = V3D(0, 0, 0.0);
+  mean_gyr      = V3D(0, 0, 0);
+  imu_need_init_    = true;
+  init_iter_num     = 1;
+  after_imu_init_   = false;
+  
+  time_last_scan = 0.0;
+}
+
+void ImuProcess::Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot)
+{
+  /** 1. initializing the gravity, gyro bias, acc and gyro covariance
+   ** 2. normalize the acceleration measurenments to unit gravity **/
+  // V3D tmp_gravity = - mean_acc / mean_acc.norm() * G_m_s2; // state_gravity;
+  M3D hat_grav;
+  hat_grav << 0.0, gravity_(2), -gravity_(1),
+              -gravity_(2), 0.0, gravity_(0),
+              gravity_(1), -gravity_(0), 0.0;
+  double align_norm = (hat_grav * tmp_gravity).norm() / gravity_.norm() / tmp_gravity.norm();
+  double align_cos = gravity_.transpose() * tmp_gravity;
+  align_cos = align_cos / gravity_.norm() / tmp_gravity.norm();
+  if (align_norm < 1e-6)
+  {
+    if (align_cos > 1e-6)
+    {
+      rot = Eye3d;
+    }
+    else
+    {
+      rot = -Eye3d;
+    }
+  }
+  else
+  {
+    V3D align_angle = hat_grav * tmp_gravity / (hat_grav * tmp_gravity).norm() * acos(align_cos); 
+    rot = Exp(align_angle(0), align_angle(1), align_angle(2));
+  }
+}
+
+void ImuProcess::IMU_init(const MeasureGroup &meas, int &N)
+{
+  /** 1. initializing the gravity, gyro bias, acc and gyro covariance
+   ** 2. normalize the acceleration measurenments to unit gravity **/
+  ROS_INFO("IMU Initializing: %.1f %%", double(N) / MAX_INI_COUNT * 100);
+  V3D cur_acc, cur_gyr;
+  
+  if (b_first_frame_)
+  {
+    Reset();
+    N = 1;
+    b_first_frame_ = false;
+    const auto &imu_acc = meas.imu.front()->linear_acceleration;
+    const auto &gyr_acc = meas.imu.front()->angular_velocity;
+    mean_acc << imu_acc.x, imu_acc.y, imu_acc.z;
+    mean_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
+  }
+
+  for (const auto &imu : meas.imu)
+  {
+    const auto &imu_acc = imu->linear_acceleration;
+    const auto &gyr_acc = imu->angular_velocity;
+    cur_acc << imu_acc.x, imu_acc.y, imu_acc.z;
+    cur_gyr << gyr_acc.x, gyr_acc.y, gyr_acc.z;
+
+    mean_acc      += (cur_acc - mean_acc) / N;
+    mean_gyr      += (cur_gyr - mean_gyr) / N;
+
+    N ++;
+  }
+}
+
+void ImuProcess::Process(const MeasureGroup &meas, PointCloudXYZI::Ptr cur_pcl_un_)
+{  
+  if (imu_en)
+  {
+    if(meas.imu.empty())  return;
+
+    if (imu_need_init_)
+    {
+      
+      {
+        /// The very first lidar frame
+        IMU_init(meas, init_iter_num);
+
+        imu_need_init_ = true;
+
+        if (init_iter_num > MAX_INI_COUNT)
+        {
+          ROS_INFO("IMU Initializing: %.1f %%", 100.0);
+          imu_need_init_ = false;
+          *cur_pcl_un_ = *(meas.lidar);
+        }
+        // *cur_pcl_un_ = *(meas.lidar);
+      }
+      return;
+    }
+    if (!after_imu_init_) after_imu_init_ = true;
+    *cur_pcl_un_ = *(meas.lidar);
+    return;
+  }
+  else
+  {
+    *cur_pcl_un_ = *(meas.lidar);
+    return;
+  }
+}

src/FAST_LIO_LOCALIZATION/src/IMU_Processing.h

@@ -0,0 +1,59 @@
+#pragma once
+#include <cmath>
+#include <math.h>
+// #include <deque>
+// #include <mutex>
+// #include <thread>
+#include <csignal>
+#include <ros/ros.h>
+// #include <so3_math.h>
+#include <Eigen/Eigen>
+// #include "Estimator.h"
+#include <common_lib.h>
+#include <pcl/common/io.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <nav_msgs/Odometry.h>
+#include <pcl/kdtree/kdtree_flann.h>
+#include <tf/transform_broadcaster.h>
+#include <eigen_conversions/eigen_msg.h>
+#include <pcl_conversions/pcl_conversions.h>
+#include <sensor_msgs/PointCloud2.h>
+
+/// *************Preconfiguration
+
+#define MAX_INI_COUNT (100)
+const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);};
+
+/// *************IMU Process and undistortion
+class ImuProcess
+{
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  ImuProcess();
+  ~ImuProcess();
+  
+  void Reset();
+  void Process(const MeasureGroup &meas, PointCloudXYZI::Ptr pcl_un_);
+  void set_gyr_cov(const V3D &scaler);
+  void set_acc_cov(const V3D &scaler);
+  void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot);
+
+  MD(12, 12) state_cov = MD(12, 12)::Identity();
+  int    lidar_type;
+  V3D    gravity_;
+  bool   imu_en;
+  V3D    mean_acc;
+  bool   imu_need_init_ = true;
+  bool   after_imu_init_ = false;
+  bool   b_first_frame_ = true;
+  double time_last_scan = 0.0;
+  V3D cov_gyr_scale = V3D(0.0001, 0.0001, 0.0001);
+  V3D cov_vel_scale = V3D(0.0001, 0.0001, 0.0001);
+
+ private:
+  void IMU_init(const MeasureGroup &meas, int &N);
+  V3D mean_gyr;
+  int init_iter_num = 1;
+};

src/FAST_LIO_LOCALIZATION/src/all_move.cpp

@@ -0,0 +1,274 @@
+#include <ros/ros.h>
+#include <tf2_ros/buffer.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <std_msgs/Bool.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <geometry_msgs/Quaternion.h>
+#include <fast_lio_localization/GoalPose.h>
+#include <fast_lio_localization/DataAI.h>
+#include <fast_lio_localization/DataNav.h>
+#include <atomic>
+#include <thread>
+#include <cmath>
+
+namespace rm_simpal_move {
+
+struct AHRS_Eulr_t {
+    float yaw;
+    float rol;
+    float pit;
+};
+
+struct Goal_t {
+    float x;
+    float y;
+    float angle;
+    float range;
+    float max_speed;
+    float tolerance;
+    bool rotor;
+};
+
+class RMSimpleMove {
+public:
+    RMSimpleMove(ros::NodeHandle nh);
+    ~RMSimpleMove();
+
+private:
+    void timer_callback(const ros::TimerEvent&);
+    void goal_pose_callback(const fast_lio_localization::GoalPoseConstPtr& msg);
+    void reach_location_callback(const std_msgs::Bool::ConstPtr& msg);
+    int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const geometry_msgs::Quaternion &quat);
+    float calc_linear_velocity(float distance, float max_speed);
+    float calc_angular_velocity(float yaw, float target_yaw);
+    bool is_goal_reached(float x, float y, float tolerance);
+    fast_lio_localization::DataAI data_ai_msg;
+
+    ros::NodeHandle nh_;
+    ros::NodeHandle private_nh_;
+    std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    ros::Timer timer_;
+    ros::Subscriber goal_pose_sub_;
+    ros::Subscriber reach_location_sub_;
+    ros::Publisher data_ai_pub_;
+    ros::Publisher data_nav_pub_;
+    std::atomic<bool> running_;
+    std::atomic<bool> goal_reached_;
+    AHRS_Eulr_t current_eulr_;
+    Goal_t goal_pose_;
+    float distant;
+    float angle;
+};
+
+RMSimpleMove::RMSimpleMove(ros::NodeHandle nh)
+    : nh_(nh), private_nh_("~"), running_(true), goal_reached_(false)
+{
+    tf_buffer_.reset(new tf2_ros::Buffer());
+    tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_));
+    tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster());
+
+    timer_ = nh_.createTimer(ros::Duration(0.01), &RMSimpleMove::timer_callback, this);
+    goal_pose_sub_ = nh_.subscribe("/goal_pose", 10, &RMSimpleMove::goal_pose_callback, this);
+    reach_location_sub_ = nh_.subscribe("/localization_ok", 10, &RMSimpleMove::reach_location_callback,this);
+    data_ai_pub_ = nh_.advertise<fast_lio_localization::DataAI>("/chassis/data_ai", 10);
+    data_nav_pub_ = nh_.advertise<fast_lio_localization::DataNav>("/chassis/data_nav", 10);
+
+    ROS_INFO("RMSimpleMove 启动！！！");
+}
+
+RMSimpleMove::~RMSimpleMove()
+{
+    running_.store(false);
+}
+void RMSimpleMove::timer_callback(const ros::TimerEvent&)
+{
+    try
+    {
+        geometry_msgs::TransformStamped trans;
+        trans = tf_buffer_->lookupTransform("bash", "car", ros::Time(0), ros::Duration(0.01));
+        geometry_msgs::TransformStamped maps;
+        maps = tf_buffer_->lookupTransform("map", "car", ros::Time(0), ros::Duration(0.01));
+        AHRS_GetEulr(&current_eulr_, trans.transform.rotation);
+
+        //ROS_INFO("pitch=%f, roll=%f, yaw=%f", current_eulr_.pit, current_eulr_.rol, current_eulr_.yaw);
+
+        // geometry_msgs::TransformStamped goal_transform;
+        // goal_transform.header.stamp = ros::Time::now();
+        // goal_transform.header.frame_id = "map";
+        // goal_transform.child_frame_id = "goal_pose";
+        // goal_transform.transform.translation.x = goal_pose_.x;
+        // goal_transform.transform.translation.y = goal_pose_.y;
+        // goal_transform.transform.translation.z = 0.0;
+        // goal_transform.transform.rotation.x = 0.0;
+        // goal_transform.transform.rotation.y = 0.0;
+        // goal_transform.transform.rotation.z = 0.0;
+        // goal_transform.transform.rotation.w = 1.0;
+
+        // tf_broadcaster_->sendTransform(goal_transform);
+
+        geometry_msgs::TransformStamped goal_in_car;
+        goal_in_car = tf_buffer_->lookupTransform("car", "goal_pose", ros::Time(0), ros::Duration(0.01));
+
+        float goal_x_in_car = goal_in_car.transform.translation.x;
+        float goal_y_in_car = goal_in_car.transform.translation.y;
+        float map_x_in_car = maps.transform.translation.x;
+        float map_y_in_car = maps.transform.translation.y;
+
+        fast_lio_localization::DataNav data_nav_msg;
+        if (is_goal_reached(goal_x_in_car, goal_y_in_car, goal_pose_.tolerance))
+        {
+            if (!goal_reached_)
+            {
+                data_nav_msg.reached = true;
+                goal_reached_ = true;
+            }
+        }
+        else
+        {
+            data_nav_msg.reached = false;
+            goal_reached_ = false;
+        }
+        data_nav_msg.x = trans.transform.translation.x;
+        data_nav_msg.y = trans.transform.translation.y;
+        data_nav_msg.yaw = current_eulr_.yaw;
+
+        data_nav_pub_.publish(data_nav_msg);
+        distant=sqrt(pow(goal_x_in_car,2)+pow(goal_y_in_car,2));
+        angle=atan2(goal_y_in_car,goal_x_in_car);
+
+        if(goal_pose_.range>0.0){
+        data_ai_msg.yaw = calc_angular_velocity(0.0,angle);
+         if(abs(angle)<0.4)
+        data_ai_msg.vx = calc_linear_velocity(goal_x_in_car, goal_pose_.max_speed);
+         else
+        data_ai_msg.vx = 0;
+        }
+        else{
+            if(goal_pose_.range==0){
+                data_ai_msg.vx = calc_linear_velocity(goal_x_in_car, goal_pose_.max_speed);
+                data_ai_msg.vy = calc_linear_velocity(goal_y_in_car, goal_pose_.max_speed);
+                data_ai_msg.yaw = calc_angular_velocity(current_eulr_.yaw, goal_pose_.angle);
+            }else{
+                data_ai_msg.yaw = calc_angular_velocity(0.0,angle);
+            }
+        }
+        data_ai_msg.pos = distant;
+        data_ai_msg.ang = 2;
+        data_ai_msg.achieve = false;
+        if(abs(angle)<0.1)
+        data_ai_msg.achieve = true;
+        if (goal_pose_.rotor)
+            data_ai_msg.notice = 0b11000000;
+        else
+            data_ai_msg.notice = 0b00000000;
+        data_ai_pub_.publish(data_ai_msg);
+        ROS_INFO("DataAI: vx=%f, vy=%f, yaw=%f,mapx=%f,mapy=%f", data_ai_msg.vx, data_ai_msg.vy, data_ai_msg.yaw,map_x_in_car,map_y_in_car);
+       
+        ROS_INFO("Goal pose relative to car: x=%f, y=%f,yaw=%f,distant=%f,circle=%f,angle=%f,pose=%f,reach=%d", 
+        trans.transform.translation.x,trans.transform.translation.y,current_eulr_.yaw,distant
+        ,goal_pose_.range,angle,data_ai_msg.pos,bool(data_ai_msg.reach));
+
+    }
+    catch (const tf2::TransformException &ex)
+    {
+        
+        ROS_WARN("Could not transform car to map: %s", ex.what());
+    }
+}
+
+void RMSimpleMove::goal_pose_callback(const fast_lio_localization::GoalPoseConstPtr& msg)
+{
+    goal_pose_.x = msg->x;
+    goal_pose_.y = msg->y;
+    goal_pose_.angle = msg->angle;
+    goal_pose_.max_speed = msg->max_speed;
+    goal_pose_.tolerance = msg->tolerance;
+    goal_pose_.rotor = msg->rotor;
+    goal_pose_.range = msg->circle;
+    //ROS_INFO("Received goal pose: x=%f, y=%f, angle=%f", msg->x, msg->y, msg->angle);
+}
+
+int8_t RMSimpleMove::AHRS_GetEulr(AHRS_Eulr_t *eulr, const geometry_msgs::Quaternion &quat)
+{
+    if (!eulr)
+        return -1;
+
+    const float sinr_cosp = 2.0f * (quat.w * quat.x + quat.y * quat.z);
+    const float cosr_cosp = 1.0f - 2.0f * (quat.x * quat.x + quat.y * quat.y);
+    eulr->pit = atan2f(sinr_cosp, cosr_cosp);
+
+    const float sinp = 2.0f * (quat.w * quat.y - quat.z * quat.x);
+    eulr->rol = fabsf(sinp) >= 1.0f ? copysignf(M_PI / 2.0f, sinp) : asinf(sinp);
+
+    const float siny_cosp = 2.0f * (quat.w * quat.z + quat.x * quat.y);
+    const float cosy_cosp = 1.0f - 2.0f * (quat.y * quat.y + quat.z * quat.z);
+    eulr->yaw = atan2f(siny_cosp, cosy_cosp);
+
+    return 0;
+}
+
+float RMSimpleMove::calc_linear_velocity(float distance, float max_speed)
+
+{
+    float k_linear = 3.3f;
+    float k_linear_sm = 9.0f;
+    float velocity;
+    if(abs(distance)>0.3){
+    velocity = k_linear * (distance-goal_pose_.range);
+    if (abs(velocity) > max_speed)
+    {
+        velocity = copysign(max_speed, velocity);
+    }
+}else{
+    velocity = k_linear_sm * (distance-goal_pose_.range);
+    if (abs(velocity) > max_speed - 2)
+    {
+        velocity = copysign(max_speed, velocity);
+    }
+
+}
+    return velocity;
+}
+
+float RMSimpleMove::calc_angular_velocity(float yaw, float target_yaw)
+{
+    float angle_diff = target_yaw - yaw;
+    if (angle_diff > M_PI)
+    {
+        angle_diff -= 2 * M_PI;
+    }
+    else if (angle_diff < -M_PI)
+    {
+        angle_diff += 2 * M_PI;
+    }
+    // float k_angular = 2.4f;
+    // float k_angular_sm = 5.4f;
+    // if(abs(angle_diff)>0.1)
+    // return k_angular * angle_diff;
+    // else
+    // return k_angular_sm * angle_diff;
+    return angle_diff;
+}
+
+bool RMSimpleMove::is_goal_reached(float x, float y, float tolerance)
+{
+    return sqrt(x * x + y * y) < tolerance;
+}
+
+void RMSimpleMove::reach_location_callback(const std_msgs::Bool::ConstPtr& msg)
+{
+    data_ai_msg.reach = msg->data;
+} // namespace rm_simpal_move
+};
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "global_position_listener");
+    ros::NodeHandle nh;
+    rm_simpal_move::RMSimpleMove simple_move(nh);
+    ros::spin();
+    return 0;
+}

src/FAST_LIO_LOCALIZATION/src/chong.cpp

@@ -0,0 +1,226 @@
+#include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2/utils.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <fast_lio_localization/GoalPose.h>
+#include <fast_lio_localization/DataAI.h>
+#include <fast_lio_localization/DataNav.h>
+#include <nav_msgs/Odometry.h>
+#include <Eigen/Core>        // 核心矩阵运算
+#include <Eigen/Geometry>    // 几何变换相关
+#include "mbot_linux_serial.h"
+struct Pose2D {
+    double x;
+    double y;
+    double yaw;
+};
+class obtain_tf {
+public:
+    obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
+        fetchTransform(); // 在构造函数中调用 fetchTransform 方法
+    }
+    double x, y, yaw;
+    double ob_x() {
+        return x;
+    }
+    double ob_y() {
+        return y;
+    }
+    double ob_yaw() {
+        return yaw;
+    }
+     bool fetchTransform() {
+        //ROS_INFO("等待变换: %s -> %s", father_frame_.c_str(), child_frame_.c_str());
+        try {
+            transform_ = tf_buffer_.lookupTransform(
+                father_frame_, 
+                child_frame_,
+                ros::Time(0), 
+                ros::Duration(10.0)
+            );
+            //ROS_INFO("变换获取成功");
+            x = transform_.transform.translation.x;
+            y = transform_.transform.translation.y;
+            yaw = suanyaw();
+            return true;
+        } catch (tf2::TransformException &ex) {
+           // ROS_ERROR("变换获取失败: %s", ex.what());
+            return false;
+        }
+    }
+
+
+private:
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
+    std::string father_frame_;
+    std::string child_frame_;
+    geometry_msgs::TransformStamped transform_;
+
+   
+    double suanyaw() {
+        tf2::Quaternion q(
+            transform_.transform.rotation.x,
+            transform_.transform.rotation.y,
+            transform_.transform.rotation.z,
+            transform_.transform.rotation.w
+        );
+        double roll, pitch, yaw;
+        tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
+        return yaw;
+    }
+};
+
+
+
+class map_to_odom {
+public:
+    map_to_odom(ros::NodeHandle nh,Pose2D pose,ros::Publisher send_odom) :nh_(nh),map_car("map","car"),map_odom("map","camera_init"),
+   pose_(pose),send_odom_(send_odom){
+        
+        updatePose(); // 在构造函数中调用 updatePose 方法
+    }
+        void send_fix() {
+        nav_msgs::Odometry fix;
+        fix.child_frame_id = "camera_init";
+        fix.header.frame_id = "map";
+        fix.header.stamp = ros::Time::now();
+        fix.pose.pose.position.x = send_x;
+        fix.pose.pose.position.y = send_y;
+        fix.pose.pose.position.z = 0;
+        fix.pose.pose.orientation = q;
+        send_odom_.publish(fix);
+        ROS_INFO("fix:x=%.6f,y=%.6f,yaw=%.6f", fix.pose.pose.position.x, fix.pose.pose.position.y,yaw_);
+    }
+    void run() {
+        ros::Rate rate(10);// 10 Hz
+        int i=0;
+        while (ros::ok()) {
+            i++;
+            updatePose();
+            send_fix();
+            if(i>4)
+            break;
+
+        }
+    }
+   
+double suan_dx(){
+    return pose2_.x - map_car.ob_x();
+}
+double suan_dy(){
+    return pose2_.y - map_car.ob_y();
+}
+double suan_dyaw(){
+    return pose2_.yaw - map_car.ob_yaw();
+}
+private:
+    ros::NodeHandle nh_;
+    ros::Publisher send_odom_;
+    obtain_tf map_car,map_odom;
+    double send_x, send_y, send_yaw, dis , x_, y_, yaw_,targe_x,targe_y,targe_yaw;
+    geometry_msgs::Quaternion q;
+    Eigen::Vector3f translation; // 平移分量
+    Eigen::Matrix4f T_translation;
+    Eigen::Matrix3f R;
+    Eigen::Matrix3f rot_matrix;
+    Pose2D pose_,pose2_;
+
+Pose2D transformPose(const Pose2D& sys2_in_sys1) {
+    // 从输入中获取sys2在sys1下的位姿
+    double x = sys2_in_sys1.x;
+    double y = sys2_in_sys1.y;
+    double yaw = sys2_in_sys1.yaw;
+
+    // 计算sys1在sys2下的位姿
+    Pose2D sys1_in_sys2;
+    
+    // 通过旋转和平移的逆变换计算新坐标
+    sys1_in_sys2.x = -x * cos(yaw) - y * sin(yaw);
+    sys1_in_sys2.y =  x * sin(yaw) - y * cos(yaw);
+    
+    // 偏航角取反（角度标准化到 [-π, π]）
+    sys1_in_sys2.yaw = -yaw;
+    
+    // 标准化角度
+    while (sys1_in_sys2.yaw > M_PI) sys1_in_sys2.yaw -= 2 * M_PI;
+    while (sys1_in_sys2.yaw < -M_PI) sys1_in_sys2.yaw += 2 * M_PI;
+
+    return sys1_in_sys2;
+}
+
+    void updatePose() {
+        pose2_ = transformPose(pose_);
+        pose2_.y -= 0.376; // 偏移量
+        pose2_.x += 0.06; // 偏移量 
+        x_ = suan_dx() + map_odom.ob_x() ;
+        y_ = suan_dy() + map_odom.ob_y();
+        yaw_ = suan_dyaw() + map_odom.ob_yaw();
+        translation = {x_,y_,0};
+        T_translation = Eigen::Matrix4f::Identity();
+        T_translation.block<3,1>(0,3) = translation;
+        R = Eigen::AngleAxisf(yaw_, Eigen::Vector3f::UnitZ());
+        Eigen::Matrix4f T_rotation = Eigen::Matrix4f::Identity();
+        T_rotation.block<3,3>(0,0) = R;
+        Eigen::Matrix4f T_map_to_odom = T_translation * T_rotation; // 先旋转后平移
+        send_x = T_map_to_odom(0, 3);
+        send_y = T_map_to_odom(1, 3);
+        rot_matrix = T_map_to_odom.block<3, 3>(0, 0);
+        Eigen::Quaternionf quat(rot_matrix);
+        q.w = quat.w();
+        q.x = quat.x();
+        q.y = quat.y();
+        q.z = quat.z();
+
+    }
+
+
+
+    geometry_msgs::Quaternion getQuaternionFromYaw(double yaw) {
+        tf2::Quaternion tf_quat;
+        tf_quat.setRPY(0, 0, yaw);
+        geometry_msgs::Quaternion msg_quat;
+        tf2::convert(tf_quat, msg_quat);
+        return msg_quat;
+    }
+};
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "fix");
+    ros::NodeHandle nh;
+    // serialInit();
+    ros::Publisher send_odom = nh.advertise<nav_msgs::Odometry>("/map_to_odom",1000);
+    double sick1,sick2,sick3;
+    bool dian,passball;
+    bool sign = true;
+    Pose2D pose;
+    pose.x = 6.67;
+    pose.y = 1.66;
+    pose.yaw = 0.032;
+    // dian=0;
+    // x=0;
+    // y=0;
+    ros::Rate r(100);
+
+    while(ros::ok()){
+    // read5float(dian,passball);
+    dian =1;
+     if(dian==1 || sign){
+        {   
+            map_to_odom map(nh,pose,send_odom);
+            map.run();
+            if(abs(map.suan_dx())>0.006 || abs(map.suan_dy())>0.006 || abs(map.suan_dyaw()   )>0.01)
+                sign = true;
+            else
+                sign = false;
+        ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
+        }
+    }else{
+        ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
+    }
+    
+    r.sleep();
+} 
+}

src/FAST_LIO_LOCALIZATION/src/chong0.cpp

@@ -0,0 +1,230 @@
+#include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/utils.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <fast_lio_localization/GoalPose.h>
+#include <fast_lio_localization/DataAI.h>
+#include <fast_lio_localization/DataNav.h>
+#include <nav_msgs/Odometry.h>
+#include <Eigen/Core>        // 核心矩阵运算
+#include <Eigen/Geometry>    // 几何变换相关
+#include "mbot_linux_serial.h"
+
+struct Pose2D {
+    double x;
+    double y;
+    double yaw;
+};
+
+class obtain_tf {
+public:
+    obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
+        fetchTransform(); // 在构造函数中调用 fetchTransform 方法
+    }
+    double x, y, yaw;
+    double ob_x() {
+        return x;
+    }
+    double ob_y() {
+        return y;
+    }
+    double ob_yaw() {
+        return yaw;
+    }
+    bool fetchTransform() {
+    if (!tf_buffer_.canTransform(father_frame_, child_frame_, ros::Time(0), ros::Duration(1.0))) {
+        ROS_WARN("等待变换: %s -> %s 不可用", father_frame_.c_str(), child_frame_.c_str());
+        return false;
+    }
+    try {
+        transform_ = tf_buffer_.lookupTransform(
+            father_frame_, 
+            child_frame_,
+            ros::Time(0)
+        );
+        x = transform_.transform.translation.x;
+        y = transform_.transform.translation.y;
+        yaw = suanyaw();
+        return true;
+    } catch (tf2::TransformException &ex) {
+        ROS_ERROR("变换获取失败: %s", ex.what());
+        return false;
+    }
+}
+
+private:
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
+    std::string father_frame_;
+    std::string child_frame_;
+    geometry_msgs::TransformStamped transform_;
+
+   
+    double suanyaw() {
+        tf2::Quaternion q(
+            transform_.transform.rotation.x,
+            transform_.transform.rotation.y,
+            transform_.transform.rotation.z,
+            transform_.transform.rotation.w
+        );
+        double roll, pitch, yaw;
+        tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
+        return yaw;
+    }
+};
+
+
+
+class map_to_odom {
+public:
+    map_to_odom(ros::NodeHandle nh,Pose2D pose,ros::Publisher send_odom) :nh_(nh),map_car("map","car"),
+    map_odom("map","camera_init"),send_odom_(send_odom),pose_(pose) {
+       
+        updatePose(); // 在构造函数中调用 updatePose 方法
+    }
+        void send_fix() {
+        nav_msgs::Odometry fix;
+        fix.child_frame_id = "camera_init";
+        fix.header.frame_id = "map";
+        fix.header.stamp = ros::Time::now();
+        fix.pose.pose.position.x = send_x;
+        fix.pose.pose.position.y = send_y;
+        fix.pose.pose.position.z = 0;
+        fix.pose.pose.orientation = q;
+        send_odom_.publish(fix);
+        ROS_INFO("fix:x=%.6f,y=%.6f,yaw=%.6f", fix.pose.pose.position.x, fix.pose.pose.position.y,yaw_);
+    }
+    void run() {
+        int i=0;
+        while (ros::ok()) {
+            i++;
+            updatePose();
+            send_fix();
+            if(i==10)
+            break;
+
+        }
+    }
+   
+double suan_dx(){
+    return pose2_.x - map_car.ob_x();
+}
+double suan_dy(){
+    return pose2_.y - map_car.ob_y();
+}
+double suan_dyaw(){
+    return pose2_.yaw - map_car.ob_yaw();
+}
+private:
+    ros::NodeHandle nh_;
+    ros::Publisher send_odom_;
+    obtain_tf map_car,map_odom;
+    double send_x, send_y, send_yaw, dis , x_, y_, yaw_;
+    geometry_msgs::Quaternion q;
+    Eigen::Vector3f translation; // 平移分量
+    Eigen::Matrix4f T_translation;
+    Eigen::Matrix3f R;
+    Eigen::Matrix3f rot_matrix;
+    Pose2D pose_,pose2_;
+
+    Pose2D transformPose(const Pose2D& sys2_in_sys1) {
+    // 从输入中获取sys2在sys1下的位姿
+    double x = sys2_in_sys1.x;
+    double y = sys2_in_sys1.y;
+    double yaw = sys2_in_sys1.yaw;
+
+    // 计算sys1在sys2下的位姿
+    Pose2D sys1_in_sys2;
+    
+    // 通过旋转和平移的逆变换计算新坐标
+    sys1_in_sys2.x = -x * cos(yaw) - y * sin(yaw);
+    sys1_in_sys2.y =  x * sin(yaw) - y * cos(yaw);
+    
+    // 偏航角取反（角度标准化到 [-π, π]）
+    sys1_in_sys2.yaw = -yaw;
+    
+    // 标准化角度
+    while (sys1_in_sys2.yaw > M_PI) sys1_in_sys2.yaw -= 2 * M_PI;
+    while (sys1_in_sys2.yaw < -M_PI) sys1_in_sys2.yaw += 2 * M_PI;
+
+    return sys1_in_sys2;
+}
+
+
+    void updatePose() {
+        pose2_ = transformPose(pose_);
+        pose2_.y -= 0.376; // 偏移量
+        pose2_.x += 0.06; // 偏移量 
+        x_ = suan_dx() + map_odom.ob_x() ;
+        y_ = suan_dy() + map_odom.ob_y();
+        yaw_ = suan_dyaw() + map_odom.ob_yaw();
+        translation = {x_,y_,0};
+        T_translation = Eigen::Matrix4f::Identity();
+        T_translation.block<3,1>(0,3) = translation;
+        R = Eigen::AngleAxisf(yaw_, Eigen::Vector3f::UnitZ());
+        Eigen::Matrix4f T_rotation = Eigen::Matrix4f::Identity();
+        T_rotation.block<3,3>(0,0) = R;
+        Eigen::Matrix4f T_map_to_odom = T_translation * T_rotation; // 先旋转后平移
+        send_x = T_map_to_odom(0, 3);
+        send_y = T_map_to_odom(1, 3);
+        rot_matrix = T_map_to_odom.block<3, 3>(0, 0);
+        Eigen::Quaternionf quat(rot_matrix);
+        q.w = quat.w();
+        q.x = quat.x();
+        q.y = quat.y();
+        q.z = quat.z();
+
+    }
+
+
+
+    geometry_msgs::Quaternion getQuaternionFromYaw(double yaw) {
+        tf2::Quaternion tf_quat;
+        tf_quat.setRPY(0, 0, yaw);
+        geometry_msgs::Quaternion msg_quat;
+        tf2::convert(tf_quat, msg_quat);
+        return msg_quat;
+    }
+};
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "fix");
+    ros::NodeHandle nh;
+    //serialInit();
+    double sick1,sick2,sick3,dian;
+    bool passball;
+    Pose2D pose;
+    bool sign = false;
+    // x=-6;
+    // y=1.5;
+    // pose.x=0;
+    // pose.y=0;
+    pose.x=6.0;
+    pose.y=2.0;
+    pose.yaw=0.00;
+    ros::Publisher send_odom = nh.advertise<nav_msgs::Odometry>("/map_to_odom", 10);
+    ros::Rate rate(20);
+   //clearSerialBuffer();
+   dian = 1;
+    while(ros::ok()){
+    //read4float(dian);
+
+    if(dian==1){
+        {   
+            map_to_odom map(nh,pose,send_odom);
+            map.run();
+            if(abs(map.suan_dx())>0.006 || abs(map.suan_dy())>0.006 || abs(map.suan_dyaw()   )>0.01)
+                sign = true;
+            else
+                sign = false;
+        ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
+        }
+    }else{
+        ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
+    }
+    rate.sleep();
+} 
+}

src/FAST_LIO_LOCALIZATION/src/commands.cpp

@@ -0,0 +1,225 @@
+#include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/utils.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <fast_lio_localization/GoalPose.h>
+#include <fast_lio_localization/DataAI.h>
+#include <fast_lio_localization/DataNav.h>
+#include <nav_msgs/Odometry.h>
+#include <Eigen/Core>        // 核心矩阵运算
+#include <Eigen/Geometry>    // 几何变换相关
+#include <std_msgs/Bool.h>
+#include <serial/serial.h>
+#include <ros/time.h>
+#include <tf/transform_broadcaster.h>
+#include <boost/asio.hpp>
+#include <geometry_msgs/Twist.h>
+#include <fast_lio_localization/underpan.h>
+#include <fast_lio_localization/position.h>
+struct Pose2D {
+    double x;
+    double y;
+    double yaw;
+};
+
+class obtain_tf {
+public:
+    obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
+        fetchTransform(); // 在构造函数中调用 fetchTransform 方法
+    }
+    double x, y, yaw;
+    double ob_x() {
+        return x;
+    }
+    double ob_y() {
+        return y;
+    }
+    double ob_yaw() {
+        return yaw;
+    }
+    bool fetchTransform() {
+    if (!tf_buffer_.canTransform(father_frame_, child_frame_, ros::Time(0), ros::Duration(1.0))) {
+        ROS_WARN("等待变换: %s -> %s 不可用", father_frame_.c_str(), child_frame_.c_str());
+        return false;
+    }
+    try {
+        transform_ = tf_buffer_.lookupTransform(
+            father_frame_, 
+            child_frame_,
+            ros::Time(0)
+        );
+        x = transform_.transform.translation.x;
+        y = transform_.transform.translation.y;
+        yaw = suanyaw();
+        return true;
+    } catch (tf2::TransformException &ex) {
+        ROS_ERROR("变换获取失败: %s", ex.what());
+        return false;
+    }
+}
+
+private:
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
+    std::string father_frame_;
+    std::string child_frame_;
+    geometry_msgs::TransformStamped transform_;
+
+   
+    double suanyaw() {
+        tf2::Quaternion q(
+            transform_.transform.rotation.x,
+            transform_.transform.rotation.y,
+            transform_.transform.rotation.z,
+            transform_.transform.rotation.w
+        );
+        double roll, pitch, yaw;
+        tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
+        return yaw;
+    }
+};
+
+
+
+
+class send_goal {
+public:
+    std::vector<fast_lio_localization::GoalPose> pose_list;
+
+    send_goal(ros::NodeHandle nh) : nh_(nh) {
+        pose_list.resize(3); 
+        initializeGoalPoses();
+    }
+    void get_r1(float x , float y) {
+        pose_list[1].x = x;
+        pose_list[1].y = y;
+        pose_list[1].angle = 0.0;
+        pose_list[1].max_speed = 6.0;
+        pose_list[1].tolerance = 0.00;
+        pose_list[1].rotor = 0.0;
+        pose_list[1].circle = -1;
+    }
+    void sendGoalPose(const fast_lio_localization::GoalPose& goal_pose) {
+        goal_pub_.publish(goal_pose);
+        geometry_msgs::TransformStamped transform;
+        transform.header.stamp = ros::Time::now();
+        transform.header.frame_id = "map";
+        transform.child_frame_id = "goal_pose";
+        
+        transform.transform.translation.x = goal_pose.x;
+        transform.transform.translation.y = goal_pose.y;
+        transform.transform.translation.z = 0.0;
+        
+        tf::Quaternion q = tf::createQuaternionFromYaw(goal_pose.angle);
+        transform.transform.rotation.x = q.x();
+        transform.transform.rotation.y = q.y();
+        transform.transform.rotation.z = q.z();
+        transform.transform.rotation.w = q.w();
+        
+        // 广播变换
+        tf_broadcaster.sendTransform(transform);
+    }
+private:
+    ros::NodeHandle nh_;
+    ros::Publisher goal_pub_;
+    tf::TransformBroadcaster tf_broadcaster;
+
+    void initializeGoalPoses() {
+        pose_list[2];
+        goal_pub_ = nh_.advertise<fast_lio_localization::GoalPose>("goal_pose", 1000);
+        // 初始化目标点
+        pose_list[0].x = 0.490;
+        pose_list[0].y = -3.228;
+        pose_list[0].angle = 0.0;
+        pose_list[0].max_speed = 6.0;
+        pose_list[0].tolerance = 0.00;
+        pose_list[0].rotor = 0.0;
+        pose_list[0].circle = -1;
+    }
+};
+
+fast_lio_localization::underpan underpan;
+fast_lio_localization::position sub_position;
+bool th_ok = false;
+void sub_underpan_callback(const fast_lio_localization::underpan::ConstPtr& msg) {
+    underpan.dian = msg->dian;
+}
+void sub_position_callback(const fast_lio_localization::position::ConstPtr& msg) {
+    sub_position.passball = msg->passball;
+    sub_position.x = msg->x;
+    sub_position.y = msg->y;
+}
+void sub_hz_ok_callback(const std_msgs::Bool::ConstPtr& msg) {
+    th_ok = msg->data;
+}
+
+int main(int argc, char **argv) {
+    ros::init(argc, argv, "commands");
+    ros::NodeHandle nh;
+    setlocale(LC_ALL,"");
+    double sick1,sick2,sick3;
+    float r1_x,r1_y,carx, cary,before_x, before_y,dx, dy;
+    before_x = 0.0;
+    before_y = 0.0;
+    obtain_tf map_car("map", "car");
+    bool dian = false, passball = false;
+    std_msgs::Bool msg;
+    int i=0;
+    ros::Subscriber sub_underpan = nh.subscribe("/underpan", 10, sub_underpan_callback);
+    ros::Subscriber sub_r1 = nh.subscribe("/position", 10, sub_position_callback);
+    ros::Subscriber sub_hz_ok = nh.subscribe("/global_localization_th_ok",10, sub_hz_ok_callback);
+    ros::Publisher pub_sign = nh.advertise<std_msgs::Bool>("/localization_ok", 10);
+    send_goal goal(nh);
+    fast_lio_localization::GoalPose goal_pose;
+    ros::Rate rate(100);
+    // clearBothSerialBuffers();
+    goal_pose = goal.pose_list[0];
+    goal.sendGoalPose(goal_pose);
+    while (ros::ok()) {
+    ros::spinOnce();
+    dian = underpan.dian;
+    passball = sub_position.passball;
+    if(dian==1 && th_ok) {
+        map_car.fetchTransform();
+        carx = map_car.ob_x();
+        cary = map_car.ob_y();
+        dx = abs(carx - before_x);
+        dy = abs(cary - before_y);
+        ROS_INFO("dx=%.2f, dy=%.2f", dx, dy);
+        if(dx < 0.02 && dy < 0.02) {
+            i++;
+            if(i > 200) {
+            msg.data = true;
+            pub_sign.publish(msg);
+            }
+        }else{
+            i = 0;
+        }
+        before_x = carx;
+        before_y = cary;
+    }else{
+        i=0;
+    }
+    if(passball) {
+        ROS_INFO("传球模式");
+        // write1float(true,map_car.ob_x(),map_car.ob_y());
+        //read1float(r1_x,r1_y);
+        r1_x = sub_position.x;
+        r1_y = sub_position.y;
+        goal.get_r1(r1_x,r1_y);
+        goal_pose = goal.pose_list[1];
+        goal.sendGoalPose(goal_pose);
+        ROS_INFO("发送目标点: x=%.2f, y=%.2f, angle=%.2f", goal_pose.x, goal_pose.y, goal_pose.angle);
+    }else {
+        // write1float(false,0,0);
+        goal_pose = goal.pose_list[0];
+        goal.sendGoalPose(goal_pose);
+        ROS_INFO("瞄框");
+    }
+ROS_INFO("dian=%d,passball=%d",dian,passball);
+    rate.sleep();
+} 
+}

src/FAST_LIO_LOCALIZATION/src/controlpid.cpp

@@ -1,198 +0,0 @@
-#include <ros/ros.h>
-#include <geometry_msgs/Twist.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <std_msgs/Int64.h>
-#include <std_msgs/String.h>
-#include <tf2/LinearMath/Quaternion.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <vector>
-#include <sensor_msgs/LaserScan.h>
-#include "mbot_linux_serial.h"
-class PIDController {
-public:
-    PIDController(double kp, double ki, double kd)
-        : kp_(kp), ki_(ki), kd_(kd), integral_(0.0), prev_error_(0.0) {}
-
-
-    double control(double setpoint, double pv) {
-        double error = setpoint - pv;
-        integral_ += error;
-        double derivative = error - prev_error_;
-        prev_error_ = error;
-        return kp_ * error + ki_ * integral_ + kd_ * derivative;
-    }//pid
-
-private:
-    double kp_, ki_, kd_, integral_, prev_error_;
-};
-
-struct TargetPose {
-    double x, y, yaw;
-};
-
-class PoseController {
-public:
-    PoseController()
-        : nh_("~"),
-          x_controller_(0.12, 0.0, 0.0),
-          y_controller_(0.15, 0.0, 0.0),
-          yaw_controller_(0.12, 0.0, 0.0),
-          robot_mode_(0) {
-        target_poses_ = {
-            {1.582, 0.00, 0.0},
-            {0.000, 1.60, -M_PI/2},
-            {0.705, 1.60, -M_PI/2},
-            {1.482, 1.60, -M_PI/2},
-            {2.234, 1.60, -M_PI/2},
-            {2.981, 1.60, -M_PI/2},
-        };
-        target_ = target_poses_[0];
-        //laser_sub = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 10, &PoseController::laser_callback, this);
-        pose_subscriber_ = nh_.subscribe("filtered_pose", 10, &PoseController::pose_callback, this);
-        cmd_pid_publisher_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_pid", 10);
-        //position_id_subscriber_ = nh_.subscribe("/position_id", 10, &PoseController::position_id_callback, this);
-        pid_status_publisher_ = nh_.advertise<std_msgs::String>("/pid_status", 10);
-        //robot_mode_subscriber_ = nh_.subscribe("robot_mode", 10, &PoseController::robot_mode_callback, this);
-        goal_subscriber_ = nh_.subscribe("/move_base_simple/goal", 10, &PoseController::goal_callback, this);
-        serial_write3float_=nh_.subscribe("cmd_vel_pid", 10, &PoseController::serial_write3double_callback, this);
-        //scan_sub_=nh_.subscribe("/scan",10,&PoseController::distant,this);
-    }
-
-private:
-    // void robot_mode_callback(const std_msgs::Int64::ConstPtr& msg) {
-    //     robot_mode_ = msg->data;
-    //     if (msg->data == 1) {
-    //         target_ = target_poses_[0];
-    //     }
-    // }
-
-    // void position_id_callback(const std_msgs::Int64::ConstPtr& msg) {
-    //     size_t id = static_cast<size_t>(msg->data);
-    //     if (id < target_poses_.size()) {
-    //         target_ = target_poses_[id];
-    //     } else {
-    //         ROS_WARN("Invalid position id: %ld", id);
-    //     }
-    // }
-
-    void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
-        double x = msg->pose.position.x;
-        double y = msg->pose.position.y;
-        tf2::Quaternion quat;
-        tf2::fromMsg(msg->pose.orientation, quat);
-        tf2::Matrix3x3 m(quat);
-        double roll, pitch, yaw;
-        m.getRPY(roll, pitch, yaw);
-        TargetPose target_local = transformTargetPose(x, y, yaw);//算位姿
-        calculateControl(target_local);//检验
-    }
-    void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
-        double x = msg->pose.position.x;
-        double y = msg->pose.position.y;
-        tf2::Quaternion quat;
-        tf2::fromMsg(msg->pose.orientation, quat);
-        tf2::Matrix3x3 m(quat);
-        double roll, pitch, yaw;
-        m.getRPY(roll, pitch, yaw);
-       target_={x,y,yaw};
-    }//目标位置位姿
-
-    TargetPose transformTargetPose(double x, double y, double yaw) {
-        TargetPose target_local;
-        target_local.x = cos(yaw) * (target_.x - x) + sin(yaw) * (target_.y - y);
-        target_local.y = -sin(yaw) * (target_.x - x) + cos(yaw) * (target_.y - y);
-        target_local.yaw = target_.yaw - yaw;
-        if (target_local.yaw > M_PI)
-            target_local.yaw -= 2 * M_PI;
-        else if (target_local.yaw < -M_PI)
-            target_local.yaw += 2 * M_PI;
-        return target_local;
-    }//算位姿
-
-    void calculateControl(const TargetPose &target_local) {
-        double error_threshold = 0.05;
-        double control_x = 0.0, control_y = 0.0, control_yaw = 0.0;
-        if (std::abs(target_local.x) > error_threshold || std::abs(target_local.y) > error_threshold || std::abs(target_local.yaw) > error_threshold) {
-            //校零
-            control_x = x_controller_.control(0.0, target_local.x);
-            control_y = y_controller_.control(0.0, target_local.y);
-            control_yaw = yaw_controller_.control(0.0, target_local.yaw);
-            control_x = std::min(control_x, 0.2);
-            control_y = std::min(control_y, 0.2);//限速
-        } else {
-            std_msgs::String msg;
-            msg.data = (robot_mode_ == 6) ? "OKK" : "OK";
-            pid_status_publisher_.publish(msg);
-        }
-
-        geometry_msgs::Twist cmd;
-        cmd.linear.x = control_x;
-        cmd.linear.y = -control_y;
-        cmd.angular.z = control_yaw;
-        cmd_pid_publisher_.publish(cmd);//发送pid
-    }
-    //发送
-    void serial_write3double_callback(const geometry_msgs::Twist::ConstPtr& msg) {
-        //write3float(msg->linear.x, msg->linear.y, msg->angular.z);
-        write3float2(msg->linear.x, msg->linear.y, 0//msg->angular.z);//lzcdxc
-        ROS_INFO("linear.x: %f, linear.y: %f, angular.z: %f", msg->linear.x, msg->linear.y, msg->angular.z);
-    }
-
-    //避障
-//     void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
-//     static const float min_distance = 0.45; // 最小安全距离，单位：米
-//     bool obstacle_detected = false;
-
-//     for (size_t i = 0; i < msg->ranges.size(); i=i+30) {
-//         float distance = msg->ranges[i];
-//         if (distance < min_distance && !std::isnan(distance)) {
-//             obstacle_detected = true;
-//             break;
-//         }
-//     }
-
-//     if (obstacle_detected) {
-//         // 处理避障逻辑
-//         ROS_WARN("stop！！！");
-//         stopRobot();
-//     } else {
-//         // 继续正常运动
-//         continueMoving();
-//     }
-// }
-void stopRobot() {
-    geometry_msgs::Twist stop_cmd;
-    stop_cmd.linear.x = 0;
-    stop_cmd.angular.z = 0;
-    cmd_pid_publisher_.publish(stop_cmd);
-}
-
-void continueMoving() {
-    // 继续向目标移动的逻辑
-}
-    ros::NodeHandle nh_;
-    ros::Subscriber pose_subscriber_;
-    ros::Subscriber goal_subscriber_;
-    ros::Publisher cmd_pid_publisher_;
-    ros::Publisher pid_status_publisher_;
-    ros::Subscriber robot_mode_subscriber_;
-    ros::Subscriber position_id_subscriber_;
-    ros::Subscriber serial_write3float_;
-    ros::Subscriber scan_sub_;
-    std::vector<TargetPose> target_poses_;
-    PIDController x_controller_, y_controller_, yaw_controller_;
-    TargetPose target_;
-    ros::Subscriber laser_sub;
-    int robot_mode_;
-    float min_distance = 0.5; // 最小安全距离，单位：米
-};
-
-int main(int argc, char **argv) {
-    ros::init(argc, argv, "pose_controller");
-    serialInit();
-    PoseController node;
-   
-    ros::spin();
-    return 0;
-    
-}

src/FAST_LIO_LOCALIZATION/src/flag.cpp

@@ -1,52 +1,85 @@
 #include <ros/ros.h>
-#include <tf/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
-#include <tf/transform_broadcaster.h>
+#include <geometry_msgs/TransformStamped.h>
-// tf chage https://zhuanlan.zhihu.com/p/340016739
+#include <tf2/LinearMath/Quaternion.h>
 
-
+int main(int argc, char** argv)
-int main(int argc, char** argv){
+{
-  ros::init(argc, argv, "flag");
+    // 初始化ROS节点
-   ros::NodeHandle nh;
+    ros::init(argc, argv, "static_tf_broadcaster");
-  ros::Publisher pose_publisher = nh.advertise<geometry_msgs::PoseStamped>("/pose_controller/filtered_pose", 10);
     ros::NodeHandle node;
-  geometry_msgs::PoseStamped pose;
-  tf::TransformListener listener;
-  tf::TransformBroadcaster broadcaster;
-  tf::Transform transform_broadcaster;
-  ros::Duration(1.0).sleep();
 
-  ros::Rate rate(1000);
+    // 创建静态坐标变换广播器
-  while (node.ok()){
+    static tf2_ros::StaticTransformBroadcaster static_broadcaster;
-    tf::StampedTransform transform_listener;
 
-    try{
+    // 创建TransformStamped消息
-      listener.lookupTransform("map", "body",  
+    geometry_msgs::TransformStamped transform;
-                               ros::Time(0), transform_listener);
+    geometry_msgs::TransformStamped transform2;
-    }
+    geometry_msgs::TransformStamped transform3;
-    catch (tf::TransformException ex){
-      ROS_ERROR("%s",ex.what());
-      ros::Duration(1.0).sleep();
-    }
-    float robot_pose_x=transform_listener.getOrigin().x();
-    float robot_pose_y=transform_listener.getOrigin().y();
-    float robot_pose_z=transform_listener.getOrigin().z();
-    float robot_oriation_x=transform_listener.getRotation().getX();
-    float robot_oriation_y=transform_listener.getRotation().getY();
-    float robot_oriation_z=transform_listener.getRotation().getZ();
-    float robot_oriation_w=transform_listener.getRotation().getW();
-    pose.header.stamp = ros::Time::now();
-    pose.header.frame_id = "now";
-    pose.pose.position.x = robot_pose_x;  
-    pose.pose.position.y = robot_pose_y;  
-    pose.pose.position.z = robot_pose_z;  
-    pose.pose.orientation.x = robot_oriation_x;  
-    pose.pose.orientation.y = robot_oriation_y;  
-    pose.pose.orientation.z = robot_oriation_z;  
-    pose.pose.orientation.w = robot_oriation_w;  
-    pose_publisher.publish(pose);
 
+    // 设置时间戳和坐标系关系
+    transform.header.stamp = ros::Time::now();
+    transform.header.frame_id = "body";  // 父坐标系
+    transform.child_frame_id = "car";    // 子坐标系
+
+    // 设置平移参数（Y轴正方向偏移0.6米）
+    transform.transform.translation.x =- 0.0;
+    transform.transform.translation.y = 0.330;
+    transform.transform.translation.z = 0.0;
+
+    // 创建四元数（绕Z轴顺时针旋转90度）
+    tf2::Quaternion q;
+    q.setRPY(0, 0, 0); // Roll, Pitch, Yaw (Z轴旋转-90度)
+    transform.transform.rotation.x = q.x();
+    transform.transform.rotation.y = q.y();
+    transform.transform.rotation.z = q.z();
+    transform.transform.rotation.w = q.w();
+
+    // 设置 body 到 map 的变换
+    transform2.header.stamp = ros::Time::now();
+    transform2.header.frame_id = "map";  // 父坐标系
+    transform2.child_frame_id = "bash"; // 子坐标系
+
+    // 设置平移参数（假设 body 在 map 中的位置）
+    transform2.transform.translation.x = 0.0;
+    transform2.transform.translation.y = 0.330;
+    transform2.transform.translation.z = 0.0;
+
+    // 创建四元数（假设 body 在 map 中没有旋转）
+    tf2::Quaternion q2;
+    q2.setRPY(0, 0, 0); // Roll, Pitch, Yaw (Z轴旋转-90度)
+    transform2.transform.rotation.x = q2.x();
+    transform2.transform.rotation.y = q2.y();
+    transform2.transform.rotation.z = q2.z();
+    transform2.transform.rotation.w = q2.w();
+
+    // // 设置 map 到 odom 的变换
+    // transform3.header.stamp = ros::Time::now();
+    // transform3.header.frame_id = "map";  // 父坐标系
+    // transform3.child_frame_id = "coordinate"; // 子坐标系
+
+    // // 设置平移参数（假设 odom 在 map 中的位置）
+    // transform3.transform.translation.x = 0.0;
+    // transform3.transform.translation.y = 0.0;
+    // transform3.transform.translation.z = 0.0;
+
+    // // 创建四元数（假设 odom 在 map 中没有旋转）
+    // tf2::Quaternion q3;
+    // q3.setRPY(0, 0, -M_PI/2); // Roll, Pitch, Yaw (无旋转)
+    // transform3.transform.rotation.x = q3.x();
+    // transform3.transform.rotation.y = q3.y();
+    // transform3.t ransform.rotation.z = q3.z();
+    // transform3.transform.rotation.w = q3.w();
+
+    // 发送变换
+    static_broadcaster.sendTransform(transform);
+    static_broadcaster.sendTransform(transform2);
+    // static_broadcaster.sendTransform(transform3);
+
+    ROS_INFO("Static transforms published");
+
+    // 保持节点活跃
+    ros::spin();
 
-    rate.sleep();
-  }
     return 0;
-};
+}

src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp

@@ -2,48 +2,31 @@
 #include <string>
 #include <unistd.h>
 
-/*===================================================================
+
-程序功能：串口通信测试程序
-程序编写：公众号：小白学移动机器人
-其他    ：如果对代码有任何疑问，可以私信小编，一定会回复的。
-=====================================================================
-------------------关注公众号，获得更多有趣的分享---------------------
-===================================================================*/
 
 using namespace std;
 using namespace boost::asio;
 //串口相关对象
-boost::system::error_code err;
+boost::system::error_code err,err2;
-boost::asio::io_service iosev;
+boost::asio::io_service iosev,iosev2;
-boost::asio::serial_port sp(iosev);
+boost::asio::serial_port sp(iosev),sp2(iosev2);            
-uint8_t usart_start = 0xFF;                 
+uint8_t usart_idf = 0x01; 
-  uint8_t usart_id = 0x06; 
+const unsigned char ender_read = 0xFD;
-  uint8_t usart_end = 0xFE;
+const unsigned char header_read = 0xFC;
 
-/********************************************************
-            串口发送接收相关常量、变量、共用体对象
-********************************************************/
-const unsigned char ender[2] = {0x0d, 0x0a};
-const unsigned char header[2] = {0x55, 0xaa};
 
-//发送左右轮速控制速度共用体
+union receiveData2
-union sendData
-{
-	short d;
-	unsigned char data[2];
-}leftVelSet,rightVelSet;
-union sendDatelf
 {
 	float d;
 	unsigned char data[4];
-}X,Y,YAW;
+};
-//接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768）
+receiveData2 sick_1,sick_2,sick_3,r1_x,r1_y,r2_x,r2_y;
-union receiveData
+union receivesign
 {
-	short d;
+    bool d;
-	unsigned char data[2];
+    unsigned char data[1];
-}leftVelNow,rightVelNow,angleNow;
+} dian_,passball_; // 用于接收sign的值
-
+// alignas(4) unsigned char dian_[4],sick_1[4],sick_2[4],sick_3[4];
 /********************************************************
 函数功能：串口参数初始化
 入口参数：无
@@ -51,7 +34,7 @@ union receiveData
 ********************************************************/
 void serialInit()
 {
-    sp.open("/dev/ttyUSB0", err);
+    sp.open("/dev/underpan", err);
     if(err){
         std::cout << "Error: " << err << std::endl;
         std::cout << "请检查您的串口/dev/ttyUSB0 是否已经准备好：\n 1.读写权限是否打开（默认不打开) \n 2.串口名称是否正确" << std::endl;
@@ -65,151 +48,63 @@ void serialInit()
 
     iosev.run();
 }
-
+void passballserialInit()
-/********************************************************
-函数功能：将对机器人的左右轮子控制速度，打包发送给下位机
-入口参数：机器人线速度、角速度
-出口参数：
-********************************************************/
-void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag)
 {
-    unsigned char buf[18] = {0};//
+    sp2.open("/dev/tor2", err2);
-    int i, length = 0;
+    if(err2){
-
+        std::cout << "Error: " << err2 << std::endl;
-    leftVelSet.d  = Left_v;//mm/s
+        std::cout << "请检查您的串口/dev/ttyUSB0 是否已经准备好：\n 1.读写权限是否打开（默认不打开) \n 2.串口名称是否正确" << std::endl;
-    rightVelSet.d = Right_v;
+        return ;
-
-    // 设置消息头
-    for(i = 0; i < 2; i++)
-        buf[i] = header[i];             //buf[0]  buf[1]
-    
-    // 设置机器人左右轮速度
-    length = 5;
-    buf[2] = length;                    //buf[2]
-    for(i = 0; i < 2; i++)
-    {
-        buf[i + 3] = leftVelSet.data[i];  //buf[3] buf[4]
-        buf[i + 5] = rightVelSet.data[i]; //buf[5] buf[6]
     }
-    // 预留控制指令
+    sp2.set_option(serial_port::baud_rate(115200));
-    buf[3 + length - 1] = ctrlFlag;       //buf[7]
+    sp2.set_option(serial_port::flow_control(serial_port::flow_control::none));
-
+    sp2.set_option(serial_port::parity(serial_port::parity::none));
-    // 设置校验值、消息尾
+    sp2.set_option(serial_port::stop_bits(serial_port::stop_bits::one));
-    buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
+    sp2.set_option(serial_port::character_size(8));    
-    buf[3 + length + 1] = ender[0];     //buf[9]
-    buf[3 + length + 2] = ender[1];     //buf[10]
-
-    // 通过串口下发数据
-    boost::asio::write(sp, boost::asio::buffer(buf));
-
-       
 
+    iosev2.run();
+}
+double get_dian(){
+    return (bool)dian_.d;
+}
+double get_sick1(){
+    return (double)sick_1.d;
+}
+double get_sick2(){
+    return (double)sick_2.d;
+}
+double get_sick3(){
+    return (double)sick_3.d;
 }
 
-void write3float(float x,float y,float yaw)
+bool read5float(bool &dian,bool &passball)
+
+
 {
-   unsigned char buf[18] = {0};//
-    int i, length = 0;
-
-    X.d  = x;//mm/s
-    Y.d = y;
-    YAW.d = yaw;
-
-    // 设置消息头
-    for(i = 0; i < 2; i++)
-        buf[i] = header[i];             //buf[0]  buf[1]
-    
-    // 设置机器人左右轮速度
-    length = 12;
-    buf[2] = length;                    //buf[2]
-    for(i = 0; i < 4; i++)
-    {
-        buf[i + 3] = X.data[i];  //3456
-        buf[i + 7] = Y.data[i]; //78910
-        buf[i + 11] = YAW.data[i];//11121314
-    }
-
-
-    // 设置校验值、消息尾
-    buf[3 + length] = getCrc8(buf, 3 + length);//buf[8]
-    buf[3 + length + 1] = ender[0];     //buf[9]
-    buf[3 + length + 2] = ender[1];     //buf[10]
-
-    // 通过串口下发数据
-    boost::asio::write(sp, boost::asio::buffer(buf));
-
-}
-void write3float2(float x,float y,float yaw)
-{   X.d  = x;//mm/s
-    Y.d = y;
-    YAW.d = yaw;
-
-   unsigned char buf[16] = {0};//
-    buf[0]=0xFF;
-    buf[1]=0x09;
-    buf[2]=0x01;
-    buf[15]=0xFE;
-    for(int i=0;i<4;i++){
-        buf[i+3]=X.data[i];
-        buf[i+7]=Y.data[i];
-        buf[i+11]=YAW.data[i];
-    }
-    boost::asio::write(sp, boost::asio::buffer(buf));
-
-}
-std::string string2hex(const std::string& input)
-{
-    static const char* const lut = "0123456789ABCDEF";
-    size_t len = input.length();
-
-    std::string output;
-    output.reserve(2 * len);
-    for (size_t i = 0; i < len; ++i)
-    {
-        const unsigned char c = input[i];
-        output.push_back(lut[c >> 4]);
-        output.push_back(lut[c & 15]);
-    }
-    return output;
-}
-
-/********************************************************
-函数功能：从下位机读取数据
-入口参数：机器人左轮轮速、右轮轮速、角度，预留控制位
-出口参数：bool
-********************************************************/
-bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag)
-{
-    char length = 0;
     unsigned char checkSum;
-    unsigned char buf[15]={0};
+    unsigned char buf[11]={0};
-    bool succeedReadFlag = false;
     //=========================================================
     // 此段代码可以读数据的结尾，进而来进行读取数据的头部
     try
     {
         boost::asio::streambuf response;
-        boost::asio::read_until(sp, response, "\r\n", err);   // 第一次分割数据 根据数据尾"\r\n"
+        boost::asio::read_until(sp, response,(char)ender_read, err); 
 
         std::string str;
         std::istream is(&response);
 
         while(response.size() != 0)
         {
-            std::getline(is, str, (char)header[0]);          // 第二次分割数据 根据数据头 这个会丢 0x55
+            std::getline(is, str, (char)header_read);         
-            // std::cout <<"筛选前："<<" {"<< string2hex(str) << "} " <<std::endl; 
+            if(str.size() == 10) 
-            // std::cout << "str size = " << str.size() << std::endl;
-            if(str.size() == 12) 
             {
-                std::string finalStr(1, header[0]);
+                std::string finalStr(1, header_read);
                 finalStr = finalStr + str;
-
+                //std::cout << "str size = " << finalStr.size() << std::endl;
-                // std::cout <<"筛选后："<<" {"<< string2hex(finalStr) << "} " <<std::endl;   
+                for (int i = 0; i < 11; i++)
-                for (size_t i = 0; i < finalStr.size(); i++)
                 {
                     buf[i] = finalStr[i];
                 }
-                succeedReadFlag = true;
                 break;
             }
             else
@@ -225,60 +120,118 @@ bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlF
     //=========================================================        
 
     // 检查信息头
-    if (buf[0]!= header[0] || buf[1] != header[1])   //buf[0] buf[1]
+
-    {
+   if (buf[1] == usart_idf)  {
-        ROS_ERROR("Received message header error!");
+    for(int i = 0; i < 4; i++)
-        return false;
+    {   dian_.data[i] = buf[i + 2]; 
+        passball_.data[i] = buf[i + 6];
     }
-    // 数据长度
+    dian = (bool)dian_.d;
-    length = buf[2];                                 //buf[2]
+    passball= (bool)passball_.d; // buf[6]
-
-    // 检查信息校验值
-    checkSum = getCrc8(buf, 3 + length);             //buf[10] 计算得出
-    if (checkSum != buf[3 + length])                 //buf[10] 串口接收
-    {
-        ROS_ERROR("Received data check sum error!");
-        return false;
-    }    
-
-    // 读取速度值
-    for(int i = 0; i < 2; i++)
-    {
-        leftVelNow.data[i]  = buf[i + 3]; //buf[3] buf[4]
-        rightVelNow.data[i] = buf[i + 5]; //buf[5] buf[6]
-        angleNow.data[i]    = buf[i + 7]; //buf[7] buf[8]
-    }
-
-    // 读取控制标志位
-    ctrlFlag = buf[9];
-    
-    Left_v  =leftVelNow.d;
-    Right_v =rightVelNow.d;
-    Angle   =angleNow.d;
     
+    if((dian==1 || dian==0)&&(passball==1 || passball==0))
     return true;
+    else
+    return false;
+}else
+{
+    return false;
+}
+ 
+}
+bool read1float(float &x, float &y)
+
+
+{
+    unsigned char checkSum;
+    unsigned char buf[11]={0};
+    //=========================================================
+    // 此段代码可以读数据的结尾，进而来进行读取数据的头部
+    try
+    {
+        boost::asio::streambuf response;
+        boost::asio::read_until(sp2, response,(char)ender_read, err2); 
+
+        std::string str;
+        std::istream is(&response);
+
+        while(response.size() != 0)
+        {
+            std::getline(is, str, (char)header_read);         
+            if(str.size() == 10) 
+            {
+                std::string finalStr(1, header_read);
+                finalStr = finalStr + str;
+                //std::cout << "str size = " << finalStr.size() << std::endl;
+                for (int i = 0; i < 11; i++)
+                {
+                    buf[i] = finalStr[i];
+                }
+                break;
+            }
+            else
+            {
+                continue;
+            }
+        }
+    }  
+    catch(boost::system::system_error &err2)
+    {
+        ROS_ERROR("read_until error");
+    } 
+    //=========================================================        
+
+    // 检查信息头
+
+   if (buf[1] == usart_idf)  {
+    for(int i = 0; i < 4; i++)
+    {   passball_.data[0] = buf[2];
+        r2_x.data[i]  = buf[i + 3];
+        r2_y.data[i] = buf[i + 7]; 
+        
+    }
+    x = (double)r2_x.d;
+    y = (double)r2_y.d;
+
+
+    return 0;
+}
+
+}
+
+void write1float(bool passball,double x, double y)
+{
+    unsigned char buf[12] = {0};
+    buf[0] = header_read; //buf[0]
+    buf[1] = usart_idf;   //buf[1]
+    buf[11] = ender_read; //buf[10]
+    buf[2] = passball;
+    for(int i = 0; i < 4; i++)
+    {
+        buf[i + 3] = r1_x.data[i];  
+        buf[i + 7] = r1_y.data[i]; 
+    }
+    boost::asio::write(sp2, boost::asio::buffer(buf));
 }
 /********************************************************
 函数功能：获得8位循环冗余校验值
 入口参数：数组地址、长度
 出口参数：校验值
 ********************************************************/
-unsigned char getCrc8(unsigned char *ptr, unsigned short len)
+void clearBothSerialBuffers() {
-{
+    boost::system::error_code ec;
-    unsigned char crc;
+    char buffer[256];
-    unsigned char i;
-    crc = 0;
-    while(len--)
-    {
-        crc ^= *ptr++;
-        for(i = 0; i < 8; i++)
-        {
-            if(crc&0x01)
-                crc=(crc>>1)^0x8C;
-            else 
-                crc >>= 1;
-        }
-    }
-    return crc;
-}
 
+    // 清空 sp 的缓冲区
+    for (int i = 0; i < 10; ++i) { // 最多尝试10次，防止死循环
+        size_t len = sp.read_some(boost::asio::buffer(buffer, sizeof(buffer)), ec);
+        if (ec || len == 0) break;
+    }
+
+    // 清空 sp2 的缓冲区
+    ec.clear();
+    for (int i = 0; i < 10; ++i) {
+        size_t len = sp2.read_some(boost::asio::buffer(buffer, sizeof(buffer)), ec);
+        if (ec || len == 0) break;
+    }
+}

src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h

@@ -8,11 +8,14 @@
 #include <nav_msgs/Odometry.h>
 #include <boost/asio.hpp>
 #include <geometry_msgs/Twist.h>
-extern void write3float2(float x,float y,float yaw);
 extern void serialInit();
-extern void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag);
+extern void passballserialInit();
-extern void write3float(float x, float y, float yaw);
+extern void clearBothSerialBuffers();
-extern bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag);
+extern bool read5float(bool &dian,bool &passball);
-unsigned char getCrc8(unsigned char *ptr, unsigned short len);
+extern bool read1float(float &x, float &y);
-
+extern void write1float(bool passball,double x, double y);
+extern double get_dian();
+extern double get_sick1();
+extern double get_sick2();
+extern double get_sick3();
 #endif

src/FAST_LIO_LOCALIZATION/src/position.cpp

@@ -0,0 +1,130 @@
+#include <ros/ros.h>
+#include <tf2/utils.h>
+#include <std_msgs/Bool.h>
+#include <serial/serial.h>
+#include <ros/time.h>
+#include <fast_lio_localization/position.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <fast_lio_localization/underpan.h>
+static serial::Serial ser;
+static std::mutex serial_mutex;
+
+class obtain_tf {
+public:
+    obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
+        fetchTransform(); // 在构造函数中调用 fetchTransform 方法
+    }
+    double x, y, yaw;
+    double ob_x() {
+        return x;
+    }
+    double ob_y() {
+        return y;
+    }
+    double ob_yaw() {
+        return yaw;
+    }
+    bool fetchTransform() {
+    if (!tf_buffer_.canTransform(father_frame_, child_frame_, ros::Time(0), ros::Duration(1.0))) {
+        ROS_WARN("等待变换: %s -> %s 不可用", father_frame_.c_str(), child_frame_.c_str());
+        return false;
+    }
+    try {
+        transform_ = tf_buffer_.lookupTransform(
+            father_frame_, 
+            child_frame_,
+            ros::Time(0)
+        );
+        x = transform_.transform.translation.x;
+        y = transform_.transform.translation.y;
+        yaw = suanyaw();
+        return true;
+    } catch (tf2::TransformException &ex) {
+        ROS_ERROR("变换获取失败: %s", ex.what());
+        return false;
+    }
+}
+
+private:
+    tf2_ros::Buffer tf_buffer_;
+    tf2_ros::TransformListener tf_listener_;
+    std::string father_frame_;
+    std::string child_frame_;
+    geometry_msgs::TransformStamped transform_;
+
+   
+    double suanyaw() {
+        tf2::Quaternion q(
+            transform_.transform.rotation.x,
+            transform_.transform.rotation.y,
+            transform_.transform.rotation.z,
+            transform_.transform.rotation.w
+        );
+        double roll, pitch, yaw;
+        tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
+        return yaw;
+    }
+};
+
+
+
+// 串口参数
+void serialInit() {
+    std::lock_guard<std::mutex> lock(serial_mutex);
+    if (ser.isOpen()) ser.close();
+    serial::Timeout to = serial::Timeout::simpleTimeout(100);
+    ser.setPort("/dev/tor1");
+    ser.setBaudrate(115200);
+    ser.setTimeout(to);
+    ser.open();
+    if (ser.isOpen()) {
+        ser.flushInput();
+        ser.flushOutput();
+        ROS_INFO("Serial port initialized and buffers cleared.");
+    } else {
+        ROS_ERROR("Failed to open serial port!tor1_position");
+    }
+}
+
+
+bool write1float(double x,double y){
+    std::lock_guard<std::mutex> lock(serial_mutex);
+    if (!ser.isOpen()) return false;
+    try {
+        // 一帧总长度=帧头1+标志1+数据8+帧尾1=11字节
+        std::string buf(11, 0);
+        buf[0] = 0xFC; // 帧头
+        buf[1] = 0x01; // 标志
+        float x_f = static_cast<float>(x);
+        float y_f = static_cast<float>(y);
+        memcpy(&buf[2], &x_f, 4);
+        memcpy(&buf[6], &y_f, 4);
+        buf[10] = 0xFD; // 帧尾
+        ser.write(buf);
+        return true;
+    } catch (...) {
+        ROS_ERROR("Failed to write data to serial port.");
+    }
+}
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "ros_serial_node");
+    ros::NodeHandle nh;
+    serialInit();
+
+    ros::Rate rate(1000);
+    double x, y;
+    bool o;
+    obtain_tf tf_ob("map", "car");
+    while (ros::ok()) {
+        tf_ob.fetchTransform(); // 获取变换
+        x = tf_ob.ob_x();
+        y = tf_ob.ob_y();
+        o = write1float(x,y);
+        ROS_INFO("x: %f, y: %f,o=%d", x, y,int(o));
+        rate.sleep();
+    }
+}

src/FAST_LIO_LOCALIZATION/src/preprocess.cpp

@@ -3,11 +3,16 @@
 #define RETURN0     0x00
 #define RETURN0AND1 0x10
 
+const bool time_list_cut_frame(PointType &x, PointType &y) {
+    return (x.curvature < y.curvature);
+}
+
 Preprocess::Preprocess()
-  :feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
+  :lidar_type(AVIA), blind(0.01), point_filter_num(1), det_range(1000)
 {
   inf_bound = 10;
   N_SCANS   = 6;
+  SCAN_RATE = 10;
   group_size = 8;
   disA = 0.01;
   disA = 0.1; // B?
@@ -34,7 +39,6 @@ Preprocess::~Preprocess() {}
 
 void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
 {
-  feature_enabled = feat_en;
   lidar_type = lid_type;
   blind = bld;
   point_filter_num = pfilt_num;
@@ -48,6 +52,25 @@ void Preprocess::process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, Poin
 
 void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
 {
+  switch (time_unit)
+  {
+    case SEC:
+      time_unit_scale = 1.e3f;
+      break;
+    case MS:
+      time_unit_scale = 1.f;
+      break;
+    case US:
+      time_unit_scale = 1.e-3f;
+      break;
+    case NS:
+      time_unit_scale = 1.e-6f;
+      break;
+    default:
+      time_unit_scale = 1.f;
+      break;
+  }
+
   switch (lidar_type)
   {
   case OUST64:
@@ -58,6 +81,10 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
     velodyne_handler(msg);
     break;
   
+  case HESAIxt32:
+    hesai_handler(msg);
+    break;
+  
   default:
     printf("Error LiDAR Type");
     break;
@@ -65,6 +92,227 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
   *pcl_out = pl_surf;
 }
 
+void Preprocess::process_cut_frame_livox(const livox_ros_driver2::CustomMsg::ConstPtr &msg,
+                                         deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar,
+                                         const int required_frame_num, int scan_count) {
+    int plsize = msg->point_num;
+    pl_surf.clear();
+    pl_surf.reserve(plsize);
+    pl_full.clear();
+    pl_full.resize(plsize);
+    int valid_point_num = 0;
+
+    for (uint i = 1; i < plsize; i++) {
+        if ((msg->points[i].line < N_SCANS) &&
+        ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
+        {
+            valid_point_num++;
+            if (valid_point_num % point_filter_num == 0) {
+                pl_full[i].x = msg->points[i].x;
+                pl_full[i].y = msg->points[i].y;
+                pl_full[i].z = msg->points[i].z;
+                pl_full[i].intensity = msg->points[i].reflectivity;
+                //use curvature as time of each laser points，unit: ms
+                pl_full[i].curvature = msg->points[i].offset_time / float(1000000);
+
+                double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;
+                if (dist < blind * blind || dist > det_range * det_range) continue;
+
+                if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)
+                    || (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)
+                    || (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) {
+                    pl_surf.push_back(pl_full[i]);
+                }
+            }
+        }
+    }
+    sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);
+    //end time of last frame，单位ms
+    double last_frame_end_time = msg->header.stamp.toSec() * 1000;
+    uint valid_num = 0;
+    uint cut_num = 0;
+    uint valid_pcl_size = pl_surf.points.size();
+
+    int required_cut_num = required_frame_num;
+    if (scan_count < 5)
+        required_cut_num = 1;
+
+    PointCloudXYZI pcl_cut;
+    for (uint i = 1; i < valid_pcl_size; i++) {
+        valid_num++;
+        //Compute new opffset time of each point：ms
+        pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time;
+        pcl_cut.push_back(pl_surf[i]);
+        if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {
+            cut_num++;
+            time_lidar.push_back(last_frame_end_time);
+            PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI()); //Initialize shared_ptr
+            *pcl_temp = pcl_cut;
+            pcl_out.push_back(pcl_temp);
+            //Update frame head
+            last_frame_end_time += pl_surf[i].curvature;
+            pcl_cut.clear();
+            pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num);
+        }
+    }
+}
+#define MAX_LINE_NUM 128
+void
+Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out,
+                                   deque<double> &time_lidar, const int required_frame_num, int scan_count) {
+    pl_surf.clear();
+    pl_corn.clear();
+    pl_full.clear();
+    if (lidar_type == VELO16) {
+        pcl::PointCloud<velodyne_ros::Point> pl_orig;
+        pcl::fromROSMsg(*msg, pl_orig);
+        int plsize = pl_orig.points.size();
+        pl_surf.reserve(plsize);
+
+        bool is_first[MAX_LINE_NUM];
+        double yaw_fp[MAX_LINE_NUM] = {0};     // yaw of first scan point
+        double omega_l = 3.61;       // scan angular velocity (deg/ms)
+        float yaw_last[MAX_LINE_NUM] = {0.0};  // yaw of last scan point
+        float time_last[MAX_LINE_NUM] = {0.0}; // last offset time
+
+        if (pl_orig.points[plsize - 1].time > 0) {
+            given_offset_time = true;
+        } else {
+            cout << "Compute offset time using constant rotation model." << endl;
+            given_offset_time = false;
+            memset(is_first, true, sizeof(is_first));
+        }
+
+        for (int i = 0; i < plsize; i++) {
+            PointType added_pt;
+            added_pt.normal_x = 0;
+            added_pt.normal_y = 0;
+            added_pt.normal_z = 0;
+            added_pt.x = pl_orig.points[i].x;
+            added_pt.y = pl_orig.points[i].y;
+            added_pt.z = pl_orig.points[i].z;
+            added_pt.intensity = pl_orig.points[i].intensity;
+            added_pt.curvature = pl_orig.points[i].time * 1000.0;  //ms
+
+            double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
+            if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))
+                continue;
+
+            if (!given_offset_time) {
+                int layer = pl_orig.points[i].ring;
+                double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+                if (is_first[layer]) {
+                    yaw_fp[layer] = yaw_angle;
+                    is_first[layer] = false;
+                    added_pt.curvature = 0.0;
+                    yaw_last[layer] = yaw_angle;
+                    time_last[layer] = added_pt.curvature;
+                    continue;
+                }
+                // compute offset time
+                if (yaw_angle <= yaw_fp[layer]) {
+                    added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
+                } else {
+                    added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
+                }
+                if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;
+
+                yaw_last[layer] = yaw_angle;
+                time_last[layer] = added_pt.curvature;
+            }
+
+
+            if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {
+                pl_surf.points.push_back(added_pt);
+            }
+        }
+    } else if (lidar_type == OUST64) {
+        pcl::PointCloud<ouster_ros::Point> pl_orig;
+        pcl::fromROSMsg(*msg, pl_orig);
+        int plsize = pl_orig.points.size();
+        pl_surf.reserve(plsize);
+        for (int i = 0; i < plsize; i++) {
+            PointType added_pt;
+            added_pt.normal_x = 0;
+            added_pt.normal_y = 0;
+            added_pt.normal_z = 0;
+            added_pt.x = pl_orig.points[i].x;
+            added_pt.y = pl_orig.points[i].y;
+            added_pt.z = pl_orig.points[i].z;
+            added_pt.intensity = pl_orig.points[i].intensity;
+            added_pt.curvature = pl_orig.points[i].t / 1e6;  //ns to ms
+
+            double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
+            if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))
+                continue;
+
+            if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {
+                pl_surf.points.push_back(added_pt);
+            }
+        }
+    } else if(lidar_type == HESAIxt32) {
+        pcl::PointCloud<hesai_ros::Point> pl_orig;
+        pcl::fromROSMsg(*msg, pl_orig);
+        int plsize = pl_orig.points.size();
+        pl_surf.reserve(plsize);
+        for (int i = 0; i < plsize; i++) {
+            PointType added_pt;
+            added_pt.normal_x = 0;
+            added_pt.normal_y = 0;
+            added_pt.normal_z = 0;
+            added_pt.x = pl_orig.points[i].x;
+            added_pt.y = pl_orig.points[i].y;
+            added_pt.z = pl_orig.points[i].z;
+            added_pt.intensity = pl_orig.points[i].intensity;
+            added_pt.curvature = (pl_orig.points[i].timestamp - msg->header.stamp.toSec()) * 1000;  //s to ms
+
+            double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
+            if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))
+                continue;
+
+            if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {
+                pl_surf.points.push_back(added_pt);
+            }
+        }
+    }else{
+        cout << "Wrong LiDAR Type!!!" << endl;
+        return;
+    }
+
+    sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);
+
+    //ms
+    double last_frame_end_time = msg->header.stamp.toSec() * 1000;
+    uint valid_num = 0;
+    uint cut_num = 0;
+    uint valid_pcl_size = pl_surf.points.size();
+
+    int required_cut_num = required_frame_num;
+
+    if (scan_count < 20)
+        required_cut_num = 1;
+
+
+    PointCloudXYZI pcl_cut;
+    for (uint i = 1; i < valid_pcl_size; i++) {
+        valid_num++;
+        pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time;
+        pcl_cut.push_back(pl_surf[i]);
+
+        if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {
+            cut_num++;
+            time_lidar.push_back(last_frame_end_time);
+            PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI());
+            *pcl_temp = pcl_cut;
+            pcl_out.push_back(pcl_temp);
+            last_frame_end_time += pl_surf[i].curvature;
+            pcl_cut.clear();
+            pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num);
+        }
+    }
+}
+
 void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
 {
   pl_surf.clear();
@@ -72,7 +320,6 @@ void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
   pl_full.clear();
   double t1 = omp_get_wtime();
   int plsize = msg->point_num;
-  // cout<<"plsie: "<<plsize<<endl;
 
   pl_corn.reserve(plsize);
   pl_surf.reserve(plsize);
@@ -85,57 +332,7 @@ void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
   }
   uint valid_num = 0;
   
-  if (feature_enabled)
-  {
-    for(uint i=1; i<plsize; i++)
-    {
-      if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
-      {
-        pl_full[i].x = msg->points[i].x;
-        pl_full[i].y = msg->points[i].y;
-        pl_full[i].z = msg->points[i].z;
-        pl_full[i].intensity = msg->points[i].reflectivity;
-        pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
   
-        bool is_new = false;
-        if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
-            || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
-            || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7))
-        {
-          pl_buff[msg->points[i].line].push_back(pl_full[i]);
-        }
-      }
-    }
-    static int count = 0;
-    static double time = 0.0;
-    count ++;
-    double t0 = omp_get_wtime();
-    for(int j=0; j<N_SCANS; j++)
-    {
-      if(pl_buff[j].size() <= 5) continue;
-      pcl::PointCloud<PointType> &pl = pl_buff[j];
-      plsize = pl.size();
-      vector<orgtype> &types = typess[j];
-      types.clear();
-      types.resize(plsize);
-      plsize--;
-      for(uint i=0; i<plsize; i++)
-      {
-        types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y;
-        vx = pl[i].x - pl[i + 1].x;
-        vy = pl[i].y - pl[i + 1].y;
-        vz = pl[i].z - pl[i + 1].z;
-        types[i].dista = vx * vx + vy * vy + vz * vz;
-      }
-      types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
-      give_feature(pl, types);
-      // pl_surf += pl;
-    }
-    time += omp_get_wtime() - t0;
-    printf("Feature extraction time: %lf \n", time / count);
-  }
-  else
-  {
   for(uint i=1; i<plsize; i++)
   {
     if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
@@ -146,23 +343,20 @@ void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
         pl_full[i].x = msg->points[i].x;
         pl_full[i].y = msg->points[i].y;
         pl_full[i].z = msg->points[i].z;
-          pl_full[i].intensity = msg->points[i].reflectivity;
+        pl_full[i].intensity = msg->points[i].reflectivity; // z; //
-          pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points
+        pl_full[i].curvature = msg->points[i].offset_time / float(1000000); // use curvature as time of each laser points, curvature unit: ms
-
+        double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;
-          if((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
+        if (dist < blind * blind || dist > det_range * det_range) continue;
+        if(((abs(pl_full[i].x - pl_full[i-1].x) > 1e-7) 
             || (abs(pl_full[i].y - pl_full[i-1].y) > 1e-7)
-              || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)
+            || (abs(pl_full[i].z - pl_full[i-1].z) > 1e-7)))
-              && (pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y > blind))
         {
-            if(pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y < max_scan_range * max_scan_range){
           pl_surf.push_back(pl_full[i]);
         }
-//            pl_surf.push_back(pl_full[i]);
-          }
-        }
       }
     }
   }
+
 }
 
 void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
@@ -175,62 +369,8 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
   int plsize = pl_orig.size();
   pl_corn.reserve(plsize);
   pl_surf.reserve(plsize);
-  if (feature_enabled)
-  {
-    for (int i = 0; i < N_SCANS; i++)
-    {
-      pl_buff[i].clear();
-      pl_buff[i].reserve(plsize);
-    }
   
-    for (uint i = 0; i < plsize; i++)
-    {
-      double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
-      if (range < blind) continue;
-      Eigen::Vector3d pt_vec;
-      PointType added_pt;
-      added_pt.x = pl_orig.points[i].x;
-      added_pt.y = pl_orig.points[i].y;
-      added_pt.z = pl_orig.points[i].z;
-      added_pt.intensity = pl_orig.points[i].intensity;
-      added_pt.normal_x = 0;
-      added_pt.normal_y = 0;
-      added_pt.normal_z = 0;
-      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
-      if (yaw_angle >= 180.0)
-        yaw_angle -= 360.0;
-      if (yaw_angle <= -180.0)
-        yaw_angle += 360.0;
   
-      added_pt.curvature = pl_orig.points[i].t / 1e6;
-      if(pl_orig.points[i].ring < N_SCANS)
-      {
-        pl_buff[pl_orig.points[i].ring].push_back(added_pt);
-      }
-    }
-
-    for (int j = 0; j < N_SCANS; j++)
-    {
-      PointCloudXYZI &pl = pl_buff[j];
-      int linesize = pl.size();
-      vector<orgtype> &types = typess[j];
-      types.clear();
-      types.resize(linesize);
-      linesize--;
-      for (uint i = 0; i < linesize; i++)
-      {
-        types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
-        vx = pl[i].x - pl[i + 1].x;
-        vy = pl[i].y - pl[i + 1].y;
-        vz = pl[i].z - pl[i + 1].z;
-        types[i].dista = vx * vx + vy * vy + vz * vz;
-      }
-      types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
-      give_feature(pl, types);
-    }
-  }
-  else
-  {
   double time_stamp = msg->header.stamp.toSec();
   // cout << "===================================" << endl;
   // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
@@ -240,7 +380,7 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
 
     double range = pl_orig.points[i].x * pl_orig.points[i].x + pl_orig.points[i].y * pl_orig.points[i].y + pl_orig.points[i].z * pl_orig.points[i].z;
     
-      if (range < blind) continue;
+    if (range < (blind * blind) || range > det_range * det_range || isnan(pl_orig.points[i].x) || isnan(pl_orig.points[i].y) || isnan(pl_orig.points[i].z)) continue;
     
     Eigen::Vector3d pt_vec;
     PointType added_pt;
@@ -251,17 +391,11 @@ void Preprocess::oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
     added_pt.normal_x = 0;
     added_pt.normal_y = 0;
     added_pt.normal_z = 0;
-      double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.3;
+    added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms
-      if (yaw_angle >= 180.0)
-        yaw_angle -= 360.0;
-      if (yaw_angle <= -180.0)
-        yaw_angle += 360.0;
-
-      added_pt.curvature = pl_orig.points[i].t / 1e6;
 
     pl_surf.points.push_back(added_pt);
   }
-  }
+  
   // pub_func(pl_surf, pub_full, msg->header.stamp);
   // pub_func(pl_surf, pub_corn, msg->header.stamp);
 }
@@ -275,10 +409,11 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
     pcl::PointCloud<velodyne_ros::Point> pl_orig;
     pcl::fromROSMsg(*msg, pl_orig);
     int plsize = pl_orig.points.size();
+    if (plsize == 0) return;
     pl_surf.reserve(plsize);
     
     /*** These variables only works when no point timestamps given ***/
-    double omega_l=3.61;       // scan angular velocity
+    double omega_l = 0.361 * SCAN_RATE;       // scan angular velocity
     std::vector<bool> is_first(N_SCANS,true);
     std::vector<double> yaw_fp(N_SCANS, 0.0);      // yaw of first scan point
     std::vector<float> yaw_last(N_SCANS, 0.0);   // yaw of last scan point
@@ -290,6 +425,111 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
       given_offset_time = true;
     }
     else
+    {
+      given_offset_time = false;
+      // double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
+      // double yaw_end  = yaw_first;
+      // int layer_first = pl_orig.points[0].ring;
+      // for (uint i = plsize - 1; i > 0; i--)
+      // {
+      //   if (pl_orig.points[i].ring == layer_first)
+      //   {
+      //     yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
+      //     break;
+      //   }
+      // }
+    }
+
+    for (int i = 0; i < plsize; i++)
+    {
+      PointType added_pt;
+      // cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
+      
+      added_pt.normal_x = 0;
+      added_pt.normal_y = 0;
+      added_pt.normal_z = 0;
+      added_pt.x = pl_orig.points[i].x;
+      added_pt.y = pl_orig.points[i].y;
+      added_pt.z = pl_orig.points[i].z;
+      added_pt.intensity = pl_orig.points[i].intensity;
+      added_pt.curvature = pl_orig.points[i].time * time_unit_scale;  // curvature unit: ms // cout<<added_pt.curvature<<endl;
+      if (i % point_filter_num != 0 || std::isnan(added_pt.x) || std::isnan(added_pt.y) || std::isnan(added_pt.z)) continue;
+
+      if (!given_offset_time)
+      {
+        // int unit_size = plsize / 16;
+        // int layer = i / unit_size; // pl_orig.points[i].ring;
+        // cout << "check layer:" << unit_size << ";" << i << ";" << layer << endl;
+        int layer = 0;
+        double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
+
+        if (is_first[layer])
+        {
+          // printf("layer: %d; is first: %d", layer, is_first[layer]);
+            yaw_fp[layer]=yaw_angle;
+            is_first[layer]=false;
+            added_pt.curvature = 0.0;
+            yaw_last[layer]=yaw_angle;
+            time_last[layer]=added_pt.curvature;
+            continue;
+        }
+
+        // compute offset time
+        if (yaw_angle < yaw_fp[layer])
+        {
+          added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
+          // added_pt.curvature = (yaw_angle + 360.0 - yaw_fp[layer]) / omega_l;
+        }
+        else
+        {
+          added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l;
+          // added_pt.curvature = (yaw_angle-yaw_fp[layer]) / omega_l;
+        }
+
+        // if (added_pt.curvature < time_last[layer])  added_pt.curvature+=360.0/omega_l;
+
+        // yaw_last[layer] = yaw_angle;
+        // time_last[layer]=added_pt.curvature;
+      }
+
+      // if (i % point_filter_num == 0 && !std::isnan(added_pt.x) && !std::isnan(added_pt.y) && !std::isnan(added_pt.z))
+      double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
+      {
+        if(dist > (blind * blind)
+        && dist < (det_range * det_range))
+        {
+          pl_surf.points.push_back(added_pt);
+        }
+      }
+    }
+    
+}
+
+void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
+{
+    pl_surf.clear();
+    pl_corn.clear();
+    pl_full.clear();
+
+    pcl::PointCloud<hesai_ros::Point> pl_orig;
+    pcl::fromROSMsg(*msg, pl_orig);
+    int plsize = pl_orig.points.size();
+    if (plsize == 0) return;
+    pl_surf.reserve(plsize);
+    
+    /*** These variables only works when no point timestamps given ***/
+    double omega_l = 0.361 * SCAN_RATE;       // scan angular velocity
+    std::vector<bool> is_first(N_SCANS,true);
+    std::vector<double> yaw_fp(N_SCANS, 0.0);      // yaw of first scan point
+    std::vector<float> yaw_last(N_SCANS, 0.0);   // yaw of last scan point
+    std::vector<float> time_last(N_SCANS, 0.0);  // last offset time
+    /*****************************************************************/
+
+    if (pl_orig.points[plsize - 1].timestamp > 0)
+    {
+      given_offset_time = true;
+    }
+    else
     {
       given_offset_time = false;
       double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
@@ -305,83 +545,8 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
       }
     }
 
-    if(feature_enabled)
+    double time_head = pl_orig.points[0].timestamp;
-    {
-      for (int i = 0; i < N_SCANS; i++)
-      {
-        pl_buff[i].clear();
-        pl_buff[i].reserve(plsize);
-      }
     
-      for (int i = 0; i < plsize; i++)
-      {
-        PointType added_pt;
-        added_pt.normal_x = 0;
-        added_pt.normal_y = 0;
-        added_pt.normal_z = 0;
-        int layer  = pl_orig.points[i].ring;
-        if (layer >= N_SCANS) continue;
-        added_pt.x = pl_orig.points[i].x;
-        added_pt.y = pl_orig.points[i].y;
-        added_pt.z = pl_orig.points[i].z;
-        added_pt.intensity = pl_orig.points[i].intensity;
-        added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms
-
-        if (!given_offset_time)
-        {
-          double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
-          if (is_first[layer])
-          {
-            // printf("layer: %d; is first: %d", layer, is_first[layer]);
-              yaw_fp[layer]=yaw_angle;
-              is_first[layer]=false;
-              added_pt.curvature = 0.0;
-              yaw_last[layer]=yaw_angle;
-              time_last[layer]=added_pt.curvature;
-              continue;
-          }
-
-          if (yaw_angle <= yaw_fp[layer])
-          {
-            added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
-          }
-          else
-          {
-            added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l;
-          }
-
-          if (added_pt.curvature < time_last[layer])  added_pt.curvature+=360.0/omega_l;
-
-          yaw_last[layer] = yaw_angle;
-          time_last[layer]=added_pt.curvature;
-        }
-
-        pl_buff[layer].points.push_back(added_pt);
-      }
-
-      for (int j = 0; j < N_SCANS; j++)
-      {
-        PointCloudXYZI &pl = pl_buff[j];
-        int linesize = pl.size();
-        if (linesize < 2) continue;
-        vector<orgtype> &types = typess[j];
-        types.clear();
-        types.resize(linesize);
-        linesize--;
-        for (uint i = 0; i < linesize; i++)
-        {
-          types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
-          vx = pl[i].x - pl[i + 1].x;
-          vy = pl[i].y - pl[i + 1].y;
-          vz = pl[i].z - pl[i + 1].z;
-          types[i].dista = vx * vx + vy * vy + vz * vz;
-        }
-        types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
-        give_feature(pl, types);
-      }
-    }
-    else
-    {
     for (int i = 0; i < plsize; i++)
     {
       PointType added_pt;
@@ -394,8 +559,7 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
       added_pt.y = pl_orig.points[i].y;
       added_pt.z = pl_orig.points[i].z;
       added_pt.intensity = pl_orig.points[i].intensity;
-        added_pt.curvature = pl_orig.points[i].time / 1000.0;
+      added_pt.curvature = (pl_orig.points[i].timestamp - time_head) * time_unit_scale;  // curvature unit: ms // cout<<added_pt.curvature<<endl;
-
       if (!given_offset_time)
       {
         int layer = pl_orig.points[i].ring;
@@ -428,15 +592,16 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
         time_last[layer]=added_pt.curvature;
       }
 
-        if (i % point_filter_num == 0)
+      if (i % point_filter_num == 0 && !std::isnan(added_pt.x) && !std::isnan(added_pt.y) && !std::isnan(added_pt.z))
       {
-          if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > blind)
+        if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > (blind * blind)
+        &&added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z < (det_range * det_range))
         {
           pl_surf.points.push_back(added_pt);
         }
       }
     }
-    }
+    
 }
 
 void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
@@ -513,52 +678,6 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
       i = i_nex;
       last_state = 0;
     }
-    // else if(plane_type == 0)
-    // {
-    //   if(last_state == 1)
-    //   {
-    //     uint i_nex_tem;
-    //     uint j;
-    //     for(j=last_i+1; j<=last_i_nex; j++)
-    //     {
-    //       uint i_nex_tem2 = i_nex_tem;
-    //       Eigen::Vector3d curr_direct2;
-
-    //       uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
-
-    //       if(ttem != 1)
-    //       {
-    //         i_nex_tem = i_nex_tem2;
-    //         break;
-    //       }
-    //       curr_direct = curr_direct2;
-    //     }
-
-    //     if(j == last_i+1)
-    //     {
-    //       last_state = 0;
-    //     }
-    //     else
-    //     {
-    //       for(uint k=last_i_nex; k<=i_nex_tem; k++)
-    //       {
-    //         if(k != i_nex_tem)
-    //         {
-    //           types[k].ftype = Real_Plane;
-    //         }
-    //         else
-    //         {
-    //           types[k].ftype = Poss_Plane;
-    //         }
-    //       }
-    //       i = i_nex_tem-1;
-    //       i_nex = i_nex_tem;
-    //       i2 = j-1;
-    //       last_state = 1;
-    //     }
-
-    //   }
-    // }
 
     last_i = i2;
     last_i_nex = i_nex;

src/FAST_LIO_LOCALIZATION/src/preprocess.h

@@ -10,11 +10,14 @@ using namespace std;
 typedef pcl::PointXYZINormal PointType;
 typedef pcl::PointCloud<PointType> PointCloudXYZI;
 
-enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
+enum LID_TYPE{AVIA = 1, VELO16, OUST64, HESAIxt32}; //{1, 2, 3, 4}
+enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3};
 enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
 enum Surround{Prev, Next};
 enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
 
+const bool time_list_cut_frame(PointType &x, PointType &y);
+
 struct orgtype
 {
   double range;
@@ -48,7 +51,25 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
     (float, z, z)
     (float, intensity, intensity)
     (float, time, time)
-    (uint16_t, ring, ring)
+    (std::uint16_t, ring, ring)
+)
+
+namespace hesai_ros {
+  struct EIGEN_ALIGN16 Point {
+      PCL_ADD_POINT4D;
+      float intensity;
+      double timestamp;
+      uint16_t ring;
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  };
+}  // namespace velodyne_ros
+POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point,
+    (float, x, x)
+    (float, y, y)
+    (float, z, z)
+    (float, intensity, intensity)
+    (double, timestamp, timestamp)
+    (std::uint16_t, ring, ring)
 )
 
 namespace ouster_ros {
@@ -86,6 +107,10 @@ class Preprocess
   Preprocess();
   ~Preprocess();
   
+  void process_cut_frame_livox(const livox_ros_driver2::CustomMsg::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
+  
+  void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
+ 
   void process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
   void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
   void set(bool feat_en, int lid_type, double bld, int pfilt_num);
@@ -94,10 +119,10 @@ class Preprocess
   PointCloudXYZI pl_full, pl_corn, pl_surf;
   PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
   vector<orgtype> typess[128]; //maximum 128 line lidar
-  int lidar_type, point_filter_num, N_SCANS;;
+  float time_unit_scale;
-  double blind;
+  int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit;
-  double max_scan_range;
+  double blind, det_range;
-  bool feature_enabled, given_offset_time;
+  bool given_offset_time;
   ros::Publisher pub_full, pub_surf, pub_corn;
     
 
@@ -105,6 +130,7 @@ class Preprocess
   void avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg);
   void oust64_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
   void velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
+  void hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg);
   void give_feature(PointCloudXYZI &pl, vector<orgtype> &types);
   void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
   int  plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);

src/FAST_LIO_LOCALIZATION/src/send.cpp

@@ -0,0 +1,195 @@
+#include <ros/ros.h>
+#include <fast_lio_localization/GoalPose.h>
+#include <tf/transform_broadcaster.h>
+#include <vector>
+#include <string>
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "send_pose");
+    ros::NodeHandle nh;
+    
+    // 创建预定义目标点列表
+    std::vector<fast_lio_localization::GoalPose> pose_list(100);
+    
+    // 初始化默认坐标点
+    // 点1：原点
+
+    // 点2：前进1米
+
+    pose_list[0].x =-3.9140;//-4.657,-4.660 4.662,4.644,4.657
+    pose_list[0].y =-5.9587;//-5.948,-5.957,-5.968,-5.965,-5.957
+    pose_list[0].angle =-1.741;
+    pose_list[0].max_speed = 5.0;
+    pose_list[0].tolerance = 0.00;
+    pose_list[0].rotor = 0.0;
+    pose_list[0].circle = -1;
+
+    pose_list[1].x =-3.9140;//-4.657,-4.660 4.662,4.644,4.657
+    pose_list[1].y =-5.9587;//-5.948,-5.957,-5.968,-5.965,-5.957
+    pose_list[1].angle =-1.741;
+    pose_list[1].max_speed = 5.0;
+    pose_list[1].tolerance = 0.00;
+    pose_list[1].rotor = 0.0;
+    pose_list[1].circle = 0;
+    // 点3：侧向移动1米，旋转90度
+
+    pose_list[2].y =-1.0;
+    pose_list[2].angle =0 ;
+    pose_list[2].max_speed = 8.0;
+    pose_list[2].tolerance = 0.00;
+    pose_list[2].rotor = 0.0;
+    pose_list[2].circle = 0.0;
+    
+
+
+    pose_list[3].x=-3.914;
+    pose_list[3].y=-5.9587;
+    pose_list[3].angle =0 ;
+    pose_list[3].max_speed = 4.0;
+    pose_list[3].tolerance = 0.00;
+    pose_list[3].rotor = 0.0;
+    pose_list[3].circle = 3.5;
+
+    pose_list[4].x=-3.914;
+    pose_list[4].y=-5.9587;
+    pose_list[4].angle =0;
+    pose_list[4].max_speed = 4.0;
+    pose_list[4].tolerance = 0.00;
+    pose_list[4].rotor = 0.0;
+    pose_list[4].circle = 3.8;
+
+    pose_list[5].x=-3.914;
+    pose_list[5].y=-5.9587;
+    pose_list[5].angle =0;
+    pose_list[5].max_speed = 4.0;
+    pose_list[5].tolerance = 0.00;
+    pose_list[5].rotor = 0.0;
+    pose_list[5].circle = 4;
+
+    pose_list[6].x=-3.914;
+    pose_list[6].y=-5.9587;
+    pose_list[6].angle =0;
+    pose_list[6].max_speed = 4.0;
+    pose_list[6].tolerance = 0.0;
+    pose_list[6].rotor = 0.0;
+    pose_list[6].circle = 4.5;
+     
+    pose_list[7].x=-3.914;
+    pose_list[7].y=-5.9587;
+    pose_list[7].angle =0;
+    pose_list[7].max_speed = 4.0;
+    pose_list[7].tolerance = 0.0;
+    pose_list[7].rotor = 0.0;
+    pose_list[7].circle = 5;
+
+    pose_list[8].x=-3.914;
+    pose_list[8].y=-5.9587;
+    pose_list[8].angle =0;
+    pose_list[8].max_speed = 4.0;
+    pose_list[8].tolerance = 0.0;
+    pose_list[8].rotor = 0.0;
+    pose_list[8].circle = 5.5;
+
+    pose_list[9].x=-3.914;
+    pose_list[9].y=-5.9587;
+    pose_list[9].angle =-1.57  ;
+    pose_list[9].max_speed = 4.0;
+    pose_list[9].tolerance = 0.0;
+    pose_list[9].rotor = 0.0;
+    pose_list[9].circle = 6.0;
+    
+    pose_list[10].x=-3.914;
+    pose_list[10].y=-5.9587;
+    pose_list[10].angle =0;
+    pose_list[10].max_speed = 4.0;
+    pose_list[10].tolerance = 0.0;
+    pose_list[10].rotor = 0.0;
+    pose_list[10].circle = 4.2;
+
+    pose_list[11].x=-3.914;
+    pose_list[11].y=-5.9587;
+    pose_list[11].angle =0;
+    pose_list[11].max_speed = 4.0;
+    pose_list[11].tolerance = 0.0;
+    pose_list[11].rotor = 0.0;
+    pose_list[11].circle = 4.7;
+
+    pose_list[12].x=-3.914;
+    pose_list[12].y=-5.9587;
+    pose_list[12].angle =0;
+    pose_list[12].max_speed = 4.0;
+    pose_list[12].tolerance = 0.0;
+    pose_list[12].rotor = 0.0;
+    pose_list[12].circle = 5.2;
+
+    pose_list[13].x=-3.914;
+    pose_list[13].y=-5.9587;
+    pose_list[13].angle =0;
+    pose_list[13].max_speed = 4.0;
+    pose_list[13].tolerance = 0.0;
+    pose_list[13].rotor = 0.0;
+    pose_list[13].circle = 5.7;
+  
+    pose_list[14].x=-3.914;
+    pose_list[14].y=-5.9587;
+    pose_list[14].angle =0;
+    pose_list[14].max_speed = 4.0;
+    pose_list[14].tolerance = 0.0;
+    pose_list[14].rotor = 0.0;
+    pose_list[14].circle = -1;
+  
+    // 参数处理
+    int selected_index = 0;  // 默认选择第一个点
+    if(argc >= 2)
+    {
+        try {
+            selected_index = std::stoi(argv[1]);
+            if(selected_index < 0 || selected_index >= pose_list.size())
+            {
+                ROS_WARN("Invalid index %d, using default pose 0", selected_index);
+                selected_index = 0;
+            }
+        }
+        catch(...) {
+            ROS_ERROR("Failed to parse index argument");
+            return 1;
+        }
+    }
+    
+    ROS_INFO("Selected pose index: %d", selected_index);
+    
+    ros::Publisher pose_publisher = nh.advertise<fast_lio_localization::GoalPose>("/goal_pose", 10);
+    tf::TransformBroadcaster tf_broadcaster;
+    ros::Rate rate(10);
+    
+    const auto& goal_pose = pose_list[selected_index];  // 获取选中的目标点
+    
+    while(ros::ok())
+    {
+        // 发布目标位姿消息
+        pose_publisher.publish(goal_pose);
+        
+        // 创建并填充坐标系变换
+        geometry_msgs::TransformStamped transform;
+        transform.header.stamp = ros::Time::now();
+        transform.header.frame_id = "map";
+        transform.child_frame_id = "goal_pose";
+        
+        transform.transform.translation.x = goal_pose.x;
+        transform.transform.translation.y = goal_pose.y;
+        transform.transform.translation.z = 0.0;
+        
+        tf::Quaternion q = tf::createQuaternionFromYaw(goal_pose.angle);
+        transform.transform.rotation.x = q.x();
+        transform.transform.rotation.y = q.y();
+        transform.transform.rotation.z = q.z();
+        transform.transform.rotation.w = q.w();
+        
+        // 广播变换
+        tf_broadcaster.sendTransform(transform);
+        rate.sleep();
+    }
+    
+    return 0;
+}

src/FAST_LIO_LOCALIZATION/src/test_goal.cpp

@@ -0,0 +1,84 @@
+#include <ros/ros.h>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/static_transform_broadcaster.h>
+#include <tf2/utils.h>  // 关键：添加四元数转换工具
+#include <geometry_msgs/PoseStamped.h>
+#include <geometry_msgs/TransformStamped.h>
+#include <fast_lio_localization/GoalPose.h>
+#include <fast_lio_localization/DataAI.h>
+#include <fast_lio_localization/DataNav.h>
+
+int main(int argc, char** argv) {
+  ros::init(argc, argv, "fixed_goal_pose_publisher");
+  ros::NodeHandle nh("~");
+
+  // 参数配置
+  std::string car_frame = "car";
+  std::string map_frame = "bash";
+  nh.param<std::string>("car", car_frame, "car");
+  nh.param<std::string>("bash", map_frame, "bash");
+
+  // 初始化TF监听器
+  tf2_ros::Buffer tf_buffer;
+  tf2_ros::TransformListener tf_listener(tf_buffer);
+
+  // 等待并获取初始位姿
+  ROS_INFO("等待 %s 到 %s 的变换...", map_frame.c_str(), car_frame.c_str());
+  geometry_msgs::TransformStamped transform;
+  try {
+    transform = tf_buffer.lookupTransform(
+      map_frame,          // target frame
+      car_frame,         // source frame
+      ros::Time(0),      // 获取最新可用时间
+      ros::Duration(10.0) // 超时时间
+    );
+    ROS_INFO("成功获取初始位姿！");
+  } catch (tf2::TransformException &ex) {
+    ROS_ERROR("变换获取失败: %s", ex.what());
+    return -1;
+  }
+
+  // 创建静态坐标系发布器
+  static tf2_ros::StaticTransformBroadcaster static_broadcaster;
+  geometry_msgs::TransformStamped static_transform;
+  static_transform.header.stamp = ros::Time::now();
+  static_transform.header.frame_id = map_frame;
+  static_transform.child_frame_id = "/goal_pose";
+  static_transform.transform = transform.transform;
+  static_broadcaster.sendTransform(static_transform);
+
+  // 关键：四元数转欧拉角计算yaw
+  tf2::Quaternion q(
+    transform.transform.rotation.x,
+    transform.transform.rotation.y,
+    transform.transform.rotation.z,
+    transform.transform.rotation.w
+  );
+  
+  double roll, pitch, yaw;
+  tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
+  
+  // 初始化话题发布器
+  ros::Publisher pose_pub = nh.advertise<fast_lio_localization::GoalPose>("/goal_pose", 10);
+
+  // 构建PoseStamped消息
+  fast_lio_localization::GoalPose pose_msg;
+  pose_msg.x = transform.transform.translation.x;
+  pose_msg.y = transform.transform.translation.y;
+  pose_msg.angle = yaw;  // 使用正确计算的yaw值
+  pose_msg.max_speed = 4.0;
+  pose_msg.circle = 0.0;
+  pose_msg.tolerance = 0.0;
+  
+  ROS_INFO("发布目标位姿: x=%.2f, y=%.2f, yaw=%.2f rad (%.1f°)", 
+          pose_msg.x, pose_msg.y, yaw, yaw * 180/M_PI);
+
+  // 持续发布话题消息
+  ros::Rate rate(10); // 10Hz
+  while (ros::ok()) {
+    pose_pub.publish(pose_msg);
+    rate.sleep();
+  }
+
+  return 0;
+}

src/FAST_LIO_LOCALIZATION/src/tor1_serial.cpp

@@ -0,0 +1,87 @@
+#include <ros/ros.h>
+#include <tf2/utils.h>
+#include <std_msgs/Bool.h>
+#include <serial/serial.h>
+#include <ros/time.h>
+#include <fast_lio_localization/position.h>
+
+static serial::Serial ser;
+static std::mutex serial_mutex;
+
+// 串口参数
+void serialInit() {
+    std::lock_guard<std::mutex> lock(serial_mutex);
+    if (ser.isOpen()) ser.close();
+    serial::Timeout to = serial::Timeout::simpleTimeout(100);
+    ser.setPort("/dev/tor1");
+    ser.setBaudrate(115200);
+    ser.setTimeout(to);
+    ser.open();
+    if (ser.isOpen()) {
+        ser.flushInput();
+        ser.flushOutput();
+        ROS_INFO("Serial port initialized and buffers cleared.");
+    } else {
+        ROS_ERROR("Failed to open serial port!tor1");
+    }
+}
+
+
+bool read2float(bool &passball, double &x, double &y) {
+    std::lock_guard<std::mutex> lock(serial_mutex);
+    try {
+        // 一帧总长度=帧头1+标志1+数据8+帧尾1=11字节
+        std::string buf = ser.read(12);
+        std::cout << "buf(hex): ";
+for (size_t i = 0; i < buf.size(); ++i) {
+    printf("%02X ", static_cast<uint8_t>(buf[i]));
+}
+std::cout << std::endl;
+        if (buf.size() != 12) return false;
+        if ((uint8_t)buf[0] != 0xFC || (uint8_t)buf[1] != 0x01 || (uint8_t)buf[11] != 0xFD)
+            return false;
+        for (size_t i = 0; i < buf.size(); ++i) {
+    printf("%02X ", static_cast<uint8_t>(buf[i]));
+}
+std::cout << std::endl;
+        bool passball_f;
+        float x_f, y_f;
+        memcpy(&passball_f, &buf[2], 1);
+        memcpy(&x_f, &buf[3], 4);
+        memcpy(&y_f, &buf[7], 4);
+        passball = bool(passball_f);
+        x=double(x_f);
+        y=double(y_f);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+fast_lio_localization::position sub_position;
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "tor1_serial_node");
+    ros::NodeHandle nh;
+    serialInit();
+    double x, y;
+    bool passball;
+    ros::Publisher pub_pos = nh.advertise<fast_lio_localization::position>("/position", 1000);
+    while (ros::ok()) {
+
+    if (read2float(passball ,x , y)) {
+        sub_position.passball = passball;
+        sub_position.x = x;
+        sub_position.y = y;
+        pub_pos.publish(sub_position);
+        ROS_INFO("pass: %d,Dian: %f, Passball: %f",int(passball),x, y);
+    } else {
+        sub_position.passball = false;
+        sub_position.x = 0.0;
+        sub_position.y = 0.0;
+        pub_pos.publish(sub_position);
+        ROS_INFO("Failed to read data from serial_2 port.");
+    }
+}
+}

src/FAST_LIO_LOCALIZATION/src/uart.cpp

@@ -0,0 +1,180 @@
+#include <ros/ros.h>
+#include <std_msgs/Float32.h>
+#include <std_msgs/Float64MultiArray.h>
+#include <string>
+#include <vector>
+#include <std_msgs/Bool.h>
+#include <fcntl.h>    // 文件控制定义
+#include <unistd.h>   // UNIX标准函数定义
+#include <termios.h>  // POSIX终端控制定义
+#include <cstring>    // 字符串操作函数
+#include <fast_lio_localization/GoalPose.h>
+#include <fast_lio_localization/DataAI.h>
+#include <fast_lio_localization/DataNav.h>
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/Twist.h>
+class SerialNode
+{
+public:
+    SerialNode()
+    {
+        ros::NodeHandle nh;
+        offsets_sub_ = nh.subscribe("/chassis/data_ai", 1000, &SerialNode::xyyawCallback, this);
+
+        // 打开串口
+        serial_fd_ = openSerialPort("/dev/r2self", B115200);
+        if (serial_fd_ == -1) {
+            ROS_ERROR("Failed to open serial port!");
+        }
+    }
+
+    ~SerialNode()
+    {
+        if (serial_fd_ != -1) {
+            close(serial_fd_);
+        }
+    }
+
+private:
+    int openSerialPort(const char* port, int baudRate)
+    {
+        // 打开串口设备
+        int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
+        if (fd == -1) {
+            ROS_ERROR_STREAM("Unable to open serial port: " << port);
+            return -1;
+        }
+
+        // 配置串口参数
+        struct termios tty;
+        memset(&tty, 0, sizeof(tty));
+
+        // 获取当前串口配置
+        if (tcgetattr(fd, &tty) != 0) {
+            ROS_ERROR_STREAM("Failed to get serial port attributes");
+            close(fd);
+            return -1;
+        }
+
+        // 设置波特率
+        cfsetospeed(&tty, baudRate);
+        cfsetispeed(&tty, baudRate);
+
+        // 设置数据位为8位
+        tty.c_cflag &= ~CSIZE;
+        tty.c_cflag |= CS8;
+
+        // 关闭奇偶校验
+        tty.c_cflag &= ~PARENB;
+
+        // 设置停止位为1位
+        tty.c_cflag &= ~CSTOPB;
+
+        // 禁用硬件流控制
+        tty.c_cflag &= ~CRTSCTS;
+
+        // 启用接收和本地模式
+        tty.c_cflag |= CREAD | CLOCAL;
+
+        // 禁用规范输入处理
+        tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+
+        // 禁用软件流控制
+        tty.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+        // 禁用输出处理
+        tty.c_oflag &= ~OPOST;
+
+        // 设置最小字符数和超时时间
+        tty.c_cc[VMIN] = 1;   // 至少读取1个字符
+        tty.c_cc[VTIME] = 10; // 超时时间为1秒
+
+        // 应用配置
+        if (tcsetattr(fd, TCSANOW, &tty) != 0) {
+            ROS_ERROR_STREAM("Failed to set serial port attributes");
+            close(fd);
+            return -1;
+        }
+
+        return fd;
+    }
+
+    void xyyawCallback(const fast_lio_localization::DataAI::ConstPtr& msg)
+    {
+       // 提取第一个目标的偏移量
+        float sent_x = static_cast<float>(msg->vx);
+        float sent_y = static_cast<float>(msg->vy);
+        float sent_yaw = static_cast<float>(msg->yaw);
+        float pos = static_cast<float>(msg->pos);
+        float ang = static_cast<float>(msg->ang);
+        bool reach = static_cast<bool>(msg->reach);
+
+        // 立即发送数据包
+        sendDataPacket(-sent_x, -sent_y, sent_yaw, pos, ang, reach);       
+
+    }
+
+    void sendDataPacket(float sent_x, float sent_y,float sent_yaw,float pos, float ang , bool reach)
+    {
+        ROS_INFO("Sending data packet...");
+        if (serial_fd_ == -1) {
+            ROS_ERROR("Serial port not open!");
+            return;
+        }
+
+        const uint8_t start_flag = 0xFF;
+        const uint8_t control_frame = 0x09;
+        const uint8_t camera_frame = 0x01;
+        const uint8_t end_flag = 0xFE;
+
+        // 转换为字节数组
+        uint8_t sent_x_bytes[4];
+        uint8_t sent_y_bytes[4];
+        uint8_t sent_yaw_bytes[4];
+        uint8_t pos_bytes[4];
+        uint8_t ang_bytes[4];
+        uint8_t reach_bytes[1];   
+        std::memcpy(sent_x_bytes, &sent_x, sizeof(sent_x));
+        std::memcpy(sent_y_bytes, &sent_y, sizeof(sent_y));
+        std::memcpy(sent_yaw_bytes, &sent_yaw, sizeof(sent_yaw));
+        std::memcpy(pos_bytes, &pos, sizeof(pos));
+        std::memcpy(ang_bytes, &ang, sizeof(ang));
+        std::memcpy(reach_bytes, &reach, sizeof(reach));
+        
+
+        // 构建数据包
+        std::vector<uint8_t> data_packet;
+        data_packet.reserve(25);  // 预分配空间优化性能
+        data_packet.push_back(start_flag);
+        data_packet.push_back(control_frame);
+        data_packet.push_back(camera_frame);
+        data_packet.insert(data_packet.end(), sent_x_bytes, sent_x_bytes + 4);
+        data_packet.insert(data_packet.end(), sent_y_bytes, sent_y_bytes + 4);
+        data_packet.insert(data_packet.end(), sent_yaw_bytes, sent_yaw_bytes + 4);
+        data_packet.insert(data_packet.end(), pos_bytes, pos_bytes + 4);
+        data_packet.insert(data_packet.end(), ang_bytes, ang_bytes + 4);
+        data_packet.insert(data_packet.end(), reach_bytes, reach_bytes + 1);
+        data_packet.push_back(end_flag);
+
+        // 发送数据
+        write(serial_fd_, data_packet.data(), data_packet.size());
+
+        //ROS_INFO("Received offsets: x=%.5f, y=%.5f, yaw=%.5f", sent_x, sent_y, sent_yaw, pos, ang, reach);
+        // 调试输出
+        //ROS_DEBUG("Sent packet: ");
+        for (auto byte : data_packet) {
+            ROS_DEBUG("%02X ", byte);
+        }
+    }
+
+    ros::Subscriber offsets_sub_;
+    int serial_fd_; // 串口文件描述符
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "serial_node");
+    SerialNode node;
+    ros::spin();
+    return 0;
+}

src/FAST_LIO_LOCALIZATION/src/underpan_serial.cpp

@@ -0,0 +1,74 @@
+#include <ros/ros.h>
+#include <tf2/utils.h>
+#include <std_msgs/Bool.h>
+#include <serial/serial.h>
+#include <ros/time.h>
+#include <fast_lio_localization/underpan.h>
+
+static serial::Serial ser;
+static std::mutex serial_mutex;
+
+// 串口参数
+void serialInit() {
+    std::lock_guard<std::mutex> lock(serial_mutex);
+    if (ser.isOpen()) ser.close();
+    serial::Timeout to = serial::Timeout::simpleTimeout(100);
+    ser.setPort("/dev/r2self");
+    ser.setBaudrate(115200);
+    ser.setTimeout(to);
+    ser.open();
+    if (ser.isOpen()) {
+        ser.flushInput();
+        ser.flushOutput();
+        ROS_INFO("Serial port initialized and buffers cleared.");
+    } else {
+        ROS_ERROR("Failed to open serial port! underpan");
+    }
+}
+
+
+bool read5float(bool &dian) {
+    std::lock_guard<std::mutex> lock(serial_mutex);
+    if (!ser.isOpen()) return false;
+    try {
+        // 一帧总长度=帧头1+标志1+数据8+帧尾1=11字节
+        std::string buf = ser.read(7);
+//         std::cout << "buf(hex): ";
+// for (size_t i = 0; i < buf.size(); ++i) {
+//     printf("%02X ", static_cast<uint8_t>(buf[i]));
+// }
+// std::cout << std::endl;
+        if (buf.size() != 7) return false;
+        if ((uint8_t)buf[0] != 0xFF || (uint8_t)buf[1] != 0x01 || (uint8_t)buf[6] != 0xFE)
+            return false;
+        float dian_f;
+        memcpy(&dian_f, &buf[2], 4);
+        dian =bool(dian_f);
+        return true;
+    } catch (...) {
+        return false;
+    }
+}
+
+int main(int argc, char** argv)
+{
+    ros::init(argc, argv, "underpan_serial_node");
+    ros::NodeHandle nh;
+    serialInit();
+    ros::Publisher pub_underpan = nh.advertise<fast_lio_localization::underpan>("/underpan", 1000);
+    fast_lio_localization::underpan underpan_msg;
+    ros::Rate loop_rate(100); //
+    bool dian = false, passball = false;
+    while (ros::ok()) {
+        if (read5float(dian)) {
+            std_msgs::Bool dian_msg, passball_msg;
+            underpan_msg.dian = dian;
+            underpan_msg.passball = passball;
+            pub_underpan.publish(underpan_msg);
+            ROS_INFO("Dian: %d, Passball: %d", dian, passball);
+        } else {
+            // ROS_INFO("Failed to read data from serial port.");
+}
+loop_rate.sleep();
+}
+}

src/Livox-SDK2

@@ -0,0 +1 @@
+Subproject commit 6a940156dd7151c3ab6a52442d86bc83613bd11b

src/Point-LIO

@@ -0,0 +1 @@
+Subproject commit 97b0042e397ec71c327966abebff9b4b7e2c154a

src/livox_ros_driver2/config/MID360_config.json

@@ -34,7 +34,7 @@
         "yaw": 0.0,
         "x": 0,
         "y": 0,
-        "z": 0
+        "z": 0.0
       }
     }
   ]

src/livox_ros_driver2/launch_ROS1/msg_MID360.launch

@@ -1,5 +1,4 @@
 <launch>
-
 	<!--user configure parameters for ros start-->
 	<arg name="lvx_file_path" default="livox_test.lvx"/>
 	<arg name="bd_list" default="100000000000000"/>
@@ -30,11 +29,12 @@
 
 	<node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
 	      type="livox_ros_driver2_node" required="true"
-	      output="screen" args="$(arg cmdline_arg)"/>
+	      args="$(arg cmdline_arg)"
+		  output="screen"/>
 
 	<group if="$(arg rosbag_enable)">
-    	<node pkg="rosbag" type="record" name="record" output="screen"
+    	<node pkg="rosbag" type="record" name="record" 
-          		args="-a"/>
+          		args="-a" output="screen"/>
     </group>
 
 </launch>

src/pcd2pgm/launch/run.launch

@@ -2,11 +2,11 @@
 <launch>
 <node pkg="pcd2pgm" name="pcd2pgm" type="pcd2pgm" output="screen">
 <!-- 存放pcd文件的路径-->
-<param name="file_directory" value= "/home/cmrt/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/" />
+<param name="file_directory" value= "/home/robofish/ws_lida/src/Point-LIO/PCD/" />i 
 <!-- pcd文件名称-->
-<param name="file_name" value= "scans" />
+<param name="file_name" value= "test" />
 <!-- 选取的范围　最小的高度-->
-<param name="thre_z_min" value= "0.1" />
+<param name="thre_z_min" value= "-0.7" />
 <!-- 选取的范围　最大的高度-->
 <param name="thre_z_max" value= "1.5" />
 <!--0 选取高度范围内的，１选取高度范围外的-->

src/pcd2pgm/src/pcd2pgm.cpp

@@ -24,8 +24,8 @@ const std::string pcd_format = ".pcd";
 
 nav_msgs::OccupancyGrid map_topic_msg;
 //最小和最大高度
-double thre_z_min = 0.3;
+double thre_z_min = 0.05;
-double thre_z_max = 2.0;
+double thre_z_max = 0.1;
 int flag_pass_through = 0;
 double map_resolution = 0.05;
 double thre_radius = 0.1;
@@ -64,8 +64,8 @@ int main(int argc, char **argv) {
 
   pcd_file = file_directory + file_name + pcd_format;
 
-  private_nh.param("thre_z_min", thre_z_min, 0.2);
+  private_nh.param("thre_z_min", thre_z_min, 0.05);
-  private_nh.param("thre_z_max", thre_z_max, 2.0);
+  private_nh.param("thre_z_max", thre_z_max, 0.1);
   private_nh.param("flag_pass_through", flag_pass_through, 0);
   private_nh.param("thre_radius", thre_radius, 0.5);
   private_nh.param("map_resolution", map_resolution, 0.05);