diff --git a/src/FAST_LIO/CMakeLists.txt b/src/FAST_LIO/CMakeLists.txt index a1ac27e..1f27df3 100644 --- a/src/FAST_LIO/CMakeLists.txt +++ b/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( diff --git a/src/FAST_LIO/config/avia.yaml b/src/FAST_LIO/config/avia.yaml index adcc03b..8c030ef 100644 --- a/src/FAST_LIO/config/avia.yaml +++ b/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 diff --git a/src/FAST_LIO/config/mid360.yaml b/src/FAST_LIO/config/mid360.yaml index 20c0e01..d503357 100644 --- a/src/FAST_LIO/config/mid360.yaml +++ b/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, diff --git a/src/FAST_LIO/launch/mapping_mid360.launch b/src/FAST_LIO/launch/mapping_mid360.launch index 0fe04fd..74c7985 100644 --- a/src/FAST_LIO/launch/mapping_mid360.launch +++ b/src/FAST_LIO/launch/mapping_mid360.launch @@ -7,13 +7,13 @@ - + - + - + diff --git a/src/FAST_LIO/src/laserMapping.cpp b/src/FAST_LIO/src/laserMapping.cpp index fbed297..2c0f5b7 100644 --- a/src/FAST_LIO/src/laserMapping.cpp +++ b/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("publish/scan_bodyframe_pub_en",scan_body_pub_en, true); nh.param("max_iteration",NUM_MAX_ITERATIONS,4); nh.param("map_file_path",map_file_path,""); - nh.param("common/lid_topic",lid_topic,"/livox/lidar"); - nh.param("common/imu_topic", imu_topic,"/livox/imu"); + nh.param("common/lid_topic",lid_topic,"livox/lidar"); + nh.param("common/imu_topic", imu_topic,"livox/imu"); nh.param("common/time_sync_en", time_sync_en, false); nh.param("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0); nh.param("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 - ("/cloud_registered", 100000); + ("cloud_registered", 100000); ros::Publisher pubLaserCloudFull_body = nh.advertise - ("/cloud_registered_body", 100000); + ("cloud_registered_body", 100000); ros::Publisher pubLaserCloudEffect = nh.advertise - ("/cloud_effected", 100000); + ("cloud_effected", 100000); ros::Publisher pubLaserCloudMap = nh.advertise - ("/Laser_map", 100000); + ("Laser_map", 100000); ros::Publisher pubOdomAftMapped = nh.advertise - ("/Odometry", 100000); + ("Odometry", 100000); ros::Publisher pubPath = nh.advertise - ("/path", 100000); + ("path", 100000); //------------------------------------------------------------------------------------------------------ signal(SIGINT, SigHandle); ros::Rate rate(5000); diff --git a/src/FAST_LIO_LOCALIZATION/.gitignore b/src/FAST_LIO_LOCALIZATION/.gitignore old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/.gitmodules b/src/FAST_LIO_LOCALIZATION/.gitmodules old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/CMakeLists.txt b/src/FAST_LIO_LOCALIZATION/CMakeLists.txt old mode 100644 new mode 100755 index 7b4cac1..d565070 --- a/src/FAST_LIO_LOCALIZATION/CMakeLists.txt +++ b/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,27 +74,67 @@ 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( - CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime + CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs message_runtime DEPENDS EIGEN3 PCL 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) -target_link_libraries(fastlio_mapping1 ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) +add_executable(uart src/uart.cpp) +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_include_directories(fastlio_mapping1 PRIVATE ${PYTHON_INCLUDE_DIRS}) +target_link_libraries(uart ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${PYTHON_LIBRARIES}) +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}) \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/LICENSE b/src/FAST_LIO_LOCALIZATION/LICENSE old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/Log/fast_lio_time_log_analysis.m b/src/FAST_LIO_LOCALIZATION/Log/fast_lio_time_log_analysis.m old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/Log/guide.md b/src/FAST_LIO_LOCALIZATION/Log/guide.md old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/Log/plot.py b/src/FAST_LIO_LOCALIZATION/Log/plot.py old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/PCD/1 b/src/FAST_LIO_LOCALIZATION/PCD/1 old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/PCD/map.pgm b/src/FAST_LIO_LOCALIZATION/PCD/map.pgm new file mode 100755 index 0000000..43028b1 Binary files /dev/null and b/src/FAST_LIO_LOCALIZATION/PCD/map.pgm differ diff --git a/src/FAST_LIO_LOCALIZATION/PCD/map.yaml b/src/FAST_LIO_LOCALIZATION/PCD/map.yaml new file mode 100755 index 0000000..3552f41 --- /dev/null +++ b/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 + diff --git a/src/FAST_LIO_LOCALIZATION/PCD/map1.pgm b/src/FAST_LIO_LOCALIZATION/PCD/map1.pgm old mode 100644 new mode 100755 index c0b3ac2..c9c7e69 Binary files a/src/FAST_LIO_LOCALIZATION/PCD/map1.pgm and b/src/FAST_LIO_LOCALIZATION/PCD/map1.pgm differ diff --git a/src/FAST_LIO_LOCALIZATION/PCD/map1.yaml b/src/FAST_LIO_LOCALIZATION/PCD/map1.yaml old mode 100644 new mode 100755 index 7af667e..a80b0e8 --- a/src/FAST_LIO_LOCALIZATION/PCD/map1.yaml +++ b/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 diff --git a/src/FAST_LIO_LOCALIZATION/PCD/map_test.pgm b/src/FAST_LIO_LOCALIZATION/PCD/map_test.pgm new file mode 100644 index 0000000..46f9af2 Binary files /dev/null and b/src/FAST_LIO_LOCALIZATION/PCD/map_test.pgm differ diff --git a/src/FAST_LIO_LOCALIZATION/PCD/map_test.yaml b/src/FAST_LIO_LOCALIZATION/PCD/map_test.yaml new file mode 100644 index 0000000..ccb4a96 --- /dev/null +++ b/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 + diff --git a/src/FAST_LIO_LOCALIZATION/PCD/mapc.pgm b/src/FAST_LIO_LOCALIZATION/PCD/mapc.pgm new file mode 100644 index 0000000..ab94a16 Binary files /dev/null and b/src/FAST_LIO_LOCALIZATION/PCD/mapc.pgm differ diff --git a/src/FAST_LIO_LOCALIZATION/PCD/mapc.yaml b/src/FAST_LIO_LOCALIZATION/PCD/mapc.yaml new file mode 100644 index 0000000..2726228 --- /dev/null +++ b/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 + diff --git a/src/FAST_LIO_LOCALIZATION/README.md b/src/FAST_LIO_LOCALIZATION/README.md old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/config/avia.yaml b/src/FAST_LIO_LOCALIZATION/config/avia.yaml old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/config/horizon.yaml b/src/FAST_LIO_LOCALIZATION/config/horizon.yaml old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/config/mid360.yaml b/src/FAST_LIO_LOCALIZATION/config/mid360.yaml old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/config/ouster64.yaml b/src/FAST_LIO_LOCALIZATION/config/ouster64.yaml old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/config/velodyne.yaml b/src/FAST_LIO_LOCALIZATION/config/velodyne.yaml old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/doc/demo.GIF b/src/FAST_LIO_LOCALIZATION/doc/demo.GIF old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/doc/demo_accu.GIF b/src/FAST_LIO_LOCALIZATION/doc/demo_accu.GIF old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/doc/demo_init.GIF b/src/FAST_LIO_LOCALIZATION/doc/demo_init.GIF old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/doc/demo_init_2.GIF b/src/FAST_LIO_LOCALIZATION/doc/demo_init_2.GIF old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/Exp_mat.h b/src/FAST_LIO_LOCALIZATION/include/Exp_mat.h old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/common_lib.h b/src/FAST_LIO_LOCALIZATION/include/common_lib.h old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/ikd-Tree/README.md b/src/FAST_LIO_LOCALIZATION/include/ikd-Tree/README.md old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.cpp b/src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.cpp old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.h b/src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.h old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/matplotlibcpp.h b/src/FAST_LIO_LOCALIZATION/include/matplotlibcpp.h old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/so3_math.h b/src/FAST_LIO_LOCALIZATION/include/so3_math.h old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/include/use-ikfom.hpp b/src/FAST_LIO_LOCALIZATION/include/use-ikfom.hpp old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch b/src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch old mode 100644 new mode 100755 index 477a902..459d3c5 --- a/src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch +++ b/src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch @@ -5,8 +5,8 @@ transform_tolerance: 0.01 - min_height: 0.0 - max_height: 1.0 + min_height: 1 + max_height: 5.0 angle_min: -3.14159 # -M_PI/2 angle_max: 3.14159 # M_PI/2 diff --git a/src/FAST_LIO_LOCALIZATION/launch/align_pcd_map.launch b/src/FAST_LIO_LOCALIZATION/launch/align_pcd_map.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/gdb_debug_example.launch b/src/FAST_LIO_LOCALIZATION/launch/gdb_debug_example.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch b/src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch old mode 100644 new mode 100755 index f198b97..bfcad41 --- a/src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch +++ b/src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch @@ -1,22 +1,21 @@ - - - - - + + + + - - + + - + @@ -38,11 +37,9 @@ - + - - diff --git a/src/FAST_LIO_LOCALIZATION/launch/localization_avia.launch b/src/FAST_LIO_LOCALIZATION/launch/localization_avia.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/localization_horizon.launch b/src/FAST_LIO_LOCALIZATION/launch/localization_horizon.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/localization_ouster64.launch b/src/FAST_LIO_LOCALIZATION/launch/localization_ouster64.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/localization_velodyne.launch b/src/FAST_LIO_LOCALIZATION/launch/localization_velodyne.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/move.launch b/src/FAST_LIO_LOCALIZATION/launch/move.launch deleted file mode 100644 index 91a7c0b..0000000 --- a/src/FAST_LIO_LOCALIZATION/launch/move.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/launch/nav.launch b/src/FAST_LIO_LOCALIZATION/launch/nav.launch old mode 100644 new mode 100755 index 826953a..8d13c1d --- a/src/FAST_LIO_LOCALIZATION/launch/nav.launch +++ b/src/FAST_LIO_LOCALIZATION/launch/nav.launch @@ -1,5 +1,7 @@ - - - + + + + \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/launch/r_serial.launch b/src/FAST_LIO_LOCALIZATION/launch/r_serial.launch new file mode 100755 index 0000000..7ace517 --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/launch/r_serial.launch @@ -0,0 +1,5 @@ + + + + \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch b/src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch old mode 100644 new mode 100755 index 3194a4b..a5acf79 --- a/src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch +++ b/src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch @@ -3,7 +3,7 @@ - + diff --git a/src/FAST_LIO_LOCALIZATION/launch/sentry_localize.launch b/src/FAST_LIO_LOCALIZATION/launch/sentry_localize.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/launch/sentry_localize_odom.launch b/src/FAST_LIO_LOCALIZATION/launch/sentry_localize_odom.launch old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/msg/DataAI.msg b/src/FAST_LIO_LOCALIZATION/msg/DataAI.msg new file mode 100755 index 0000000..03020c0 --- /dev/null +++ b/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 diff --git a/src/FAST_LIO_LOCALIZATION/msg/DataMCU.msg b/src/FAST_LIO_LOCALIZATION/msg/DataMCU.msg new file mode 100755 index 0000000..d3351fd --- /dev/null +++ b/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 \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/msg/DataNav.msg b/src/FAST_LIO_LOCALIZATION/msg/DataNav.msg new file mode 100755 index 0000000..f7c39af --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/msg/DataNav.msg @@ -0,0 +1,5 @@ +bool reached + +float32 x +float32 y +float32 yaw \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/msg/DataRef.msg b/src/FAST_LIO_LOCALIZATION/msg/DataRef.msg new file mode 100755 index 0000000..181dd7f --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/msg/DataRef.msg @@ -0,0 +1,3 @@ +uint16 remain_hp +uint8 game_progress +uint16 stage_remain_time \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/msg/GoalPose.msg b/src/FAST_LIO_LOCALIZATION/msg/GoalPose.msg new file mode 100755 index 0000000..1a6618e --- /dev/null +++ b/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 diff --git a/src/FAST_LIO_LOCALIZATION/msg/Pose6D.msg b/src/FAST_LIO_LOCALIZATION/msg/Pose6D.msg old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/msg/Ps2Data.msg b/src/FAST_LIO_LOCALIZATION/msg/Ps2Data.msg new file mode 100755 index 0000000..2b52c1c --- /dev/null +++ b/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 + diff --git a/src/FAST_LIO_LOCALIZATION/msg/position.msg b/src/FAST_LIO_LOCALIZATION/msg/position.msg new file mode 100755 index 0000000..aaaf333 --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/msg/position.msg @@ -0,0 +1,3 @@ +int8 passball +float32 x +float32 y \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/msg/serial.msg b/src/FAST_LIO_LOCALIZATION/msg/serial.msg new file mode 100755 index 0000000..c6baf4f --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/msg/serial.msg @@ -0,0 +1 @@ +uint8[] data # 定义二进制数据字段 \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/msg/underpan.msg b/src/FAST_LIO_LOCALIZATION/msg/underpan.msg new file mode 100755 index 0000000..8341a54 --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/msg/underpan.msg @@ -0,0 +1,2 @@ +int8 dian +int8 passball \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/package.xml b/src/FAST_LIO_LOCALIZATION/package.xml old mode 100644 new mode 100755 index e780a3b..5d22a1a --- a/src/FAST_LIO_LOCALIZATION/package.xml +++ b/src/FAST_LIO_LOCALIZATION/package.xml @@ -26,6 +26,7 @@ tf pcl_ros livox_ros_driver2 + serial message_generation geometry_msgs @@ -37,11 +38,12 @@ tf pcl_ros livox_ros_driver2 + serial message_runtime rostest rosbag - + diff --git a/src/FAST_LIO_LOCALIZATION/rviz_cfg/.gitignore b/src/FAST_LIO_LOCALIZATION/rviz_cfg/.gitignore old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/rviz_cfg/loam_livox.rviz b/src/FAST_LIO_LOCALIZATION/rviz_cfg/loam_livox.rviz old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/rviz_cfg/localization.rviz b/src/FAST_LIO_LOCALIZATION/rviz_cfg/localization.rviz old mode 100644 new mode 100755 index e2f6a61..2c0c4c0 --- a/src/FAST_LIO_LOCALIZATION/rviz_cfg/localization.rviz +++ b/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,14 +466,15 @@ 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 - Alpha: 1 @@ -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 diff --git a/src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_build_map.rviz b/src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_build_map.rviz old mode 100644 new mode 100755 index 5c472a2..8a277a9 --- a/src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_build_map.rviz +++ b/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 diff --git a/src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_localize.rviz b/src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_localize.rviz old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/scripts/global_localization.py b/src/FAST_LIO_LOCALIZATION/scripts/global_localization.py index 322e6b8..42f1092 100755 --- a/src/FAST_LIO_LOCALIZATION/scripts/global_localization.py +++ b/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) - pc = np.zeros([len(pc_array), 3]) - pc[:, 0] = pc_array['x'] - pc[:, 1] = pc_array['y'] - pc[:, 2] = pc_array['z'] - return pc + points_list = [] + for point in read_points(pc_msg, field_names=("x", "y", "z"), skip_nans=True): + points_list.append(point) + return np.array(points_list) 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=[ - ('x', np.float32), - ('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 + # 创建 PointCloud2 消息 + msg = create_cloud_xyz32(header, pc) 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) + # 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了 - global_localization(T_map_to_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() + rospy.spin() \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/scripts/publish_initial_pose.py b/src/FAST_LIO_LOCALIZATION/scripts/publish_initial_pose.py index 4f17fac..ee021e4 100755 --- a/src/FAST_LIO_LOCALIZATION/scripts/publish_initial_pose.py +++ b/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 diff --git a/src/FAST_LIO_LOCALIZATION/src/.vscode/c_cpp_properties.json b/src/FAST_LIO_LOCALIZATION/src/.vscode/c_cpp_properties.json new file mode 100755 index 0000000..fd7508e --- /dev/null +++ b/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 +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/.vscode/settings.json b/src/FAST_LIO_LOCALIZATION/src/.vscode/settings.json new file mode 100755 index 0000000..5aafb1d --- /dev/null +++ b/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" + ] +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/IMU_Processing.cpp b/src/FAST_LIO_LOCALIZATION/src/IMU_Processing.cpp new file mode 100755 index 0000000..0f51d2e --- /dev/null +++ b/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; + } +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/IMU_Processing.h b/src/FAST_LIO_LOCALIZATION/src/IMU_Processing.h new file mode 100755 index 0000000..a7f888e --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/IMU_Processing.h @@ -0,0 +1,59 @@ +#pragma once +#include +#include +// #include +// #include +// #include +#include +#include +// #include +#include +// #include "Estimator.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/// *************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; +}; diff --git a/src/FAST_LIO_LOCALIZATION/src/IMU_Processing.hpp b/src/FAST_LIO_LOCALIZATION/src/IMU_Processing.hpp old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/src/all_move.cpp b/src/FAST_LIO_LOCALIZATION/src/all_move.cpp new file mode 100755 index 0000000..e1b9d0b --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/all_move.cpp @@ -0,0 +1,274 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +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 tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr 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 running_; + std::atomic 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("/chassis/data_ai", 10); + data_nav_pub_ = nh_.advertise("/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(¤t_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; +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/chong.cpp b/src/FAST_LIO_LOCALIZATION/src/chong.cpp new file mode 100755 index 0000000..fd269d1 --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/chong.cpp @@ -0,0 +1,226 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // 核心矩阵运算 +#include // 几何变换相关 +#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("/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(); +} +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/chong0.cpp b/src/FAST_LIO_LOCALIZATION/src/chong0.cpp new file mode 100755 index 0000000..5ae8a2a --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/chong0.cpp @@ -0,0 +1,230 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // 核心矩阵运算 +#include // 几何变换相关 +#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("/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(); +} +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/commands.cpp b/src/FAST_LIO_LOCALIZATION/src/commands.cpp new file mode 100755 index 0000000..b93d02c --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/commands.cpp @@ -0,0 +1,225 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // 核心矩阵运算 +#include // 几何变换相关 +#include +#include +#include +#include +#include +#include +#include +#include +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 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("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("/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(); +} +} diff --git a/src/FAST_LIO_LOCALIZATION/src/controlpid.cpp b/src/FAST_LIO_LOCALIZATION/src/controlpid.cpp deleted file mode 100644 index 15a4649..0000000 --- a/src/FAST_LIO_LOCALIZATION/src/controlpid.cpp +++ /dev/null @@ -1,198 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#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("/scan", 10, &PoseController::laser_callback, this); - pose_subscriber_ = nh_.subscribe("filtered_pose", 10, &PoseController::pose_callback, this); - cmd_pid_publisher_ = nh_.advertise("cmd_vel_pid", 10); - //position_id_subscriber_ = nh_.subscribe("/position_id", 10, &PoseController::position_id_callback, this); - pid_status_publisher_ = nh_.advertise("/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(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 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; - -} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/flag.cpp b/src/FAST_LIO_LOCALIZATION/src/flag.cpp old mode 100644 new mode 100755 index 5cbf182..eb79241 --- a/src/FAST_LIO_LOCALIZATION/src/flag.cpp +++ b/src/FAST_LIO_LOCALIZATION/src/flag.cpp @@ -1,52 +1,85 @@ #include -#include -#include -// tf chage https://zhuanlan.zhihu.com/p/340016739 +#include +#include +#include +int main(int argc, char** argv) +{ + // 初始化ROS节点 + ros::init(argc, argv, "static_tf_broadcaster"); + ros::NodeHandle node; -int main(int argc, char** argv){ - ros::init(argc, argv, "flag"); - ros::NodeHandle nh; - ros::Publisher pose_publisher = nh.advertise("/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(); + // 创建静态坐标变换广播器 + static tf2_ros::StaticTransformBroadcaster static_broadcaster; - ros::Rate rate(1000); - while (node.ok()){ - tf::StampedTransform transform_listener; - - try{ - listener.lookupTransform("map", "body", - ros::Time(0), transform_listener); - } - 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); - + // 创建TransformStamped消息 + geometry_msgs::TransformStamped transform; + geometry_msgs::TransformStamped transform2; + geometry_msgs::TransformStamped transform3; - rate.sleep(); - } - return 0; -}; \ No newline at end of file + // 设置时间戳和坐标系关系 + 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(); + + return 0; +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/laserMapping.cpp b/src/FAST_LIO_LOCALIZATION/src/laserMapping.cpp old mode 100644 new mode 100755 diff --git a/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp b/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp old mode 100644 new mode 100755 index 2bad270..5edac0e --- a/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp +++ b/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp @@ -2,48 +2,31 @@ #include #include -/*=================================================================== -程序功能：串口通信测试程序 -程序编写：公众号：小白学移动机器人 -其他 ：如果对代码有任何疑问，可以私信小编，一定会回复的。 -===================================================================== -------------------关注公众号，获得更多有趣的分享--------------------- -===================================================================*/ + using namespace std; using namespace boost::asio; //串口相关对象 -boost::system::error_code err; -boost::asio::io_service iosev; -boost::asio::serial_port sp(iosev); -uint8_t usart_start = 0xFF; - uint8_t usart_id = 0x06; - uint8_t usart_end = 0xFE; +boost::system::error_code err,err2; +boost::asio::io_service iosev,iosev2; +boost::asio::serial_port sp(iosev),sp2(iosev2); +uint8_t usart_idf = 0x01; +const unsigned char ender_read = 0xFD; +const unsigned char header_read = 0xFC; -/******************************************************** - 串口发送接收相关常量、变量、共用体对象 -********************************************************/ -const unsigned char ender[2] = {0x0d, 0x0a}; -const unsigned char header[2] = {0x55, 0xaa}; -//发送左右轮速控制速度共用体 -union sendData +union receiveData2 { - short d; - unsigned char data[2]; -}leftVelSet,rightVelSet; -union sendDatelf + float d; + unsigned char data[4]; +}; +receiveData2 sick_1,sick_2,sick_3,r1_x,r1_y,r2_x,r2_y; +union receivesign { - float d; - unsigned char data[4]; -}X,Y,YAW; -//接收数据（左轮速、右轮速、角度）共用体（-32767 - +32768） -union receiveData -{ - short d; - unsigned char data[2]; -}leftVelNow,rightVelNow,angleNow; - + bool d; + unsigned char data[1]; +} 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 writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag) +void passballserialInit() { - unsigned char buf[18] = {0};// - int i, length = 0; - - leftVelSet.d = Left_v;//mm/s - rightVelSet.d = Right_v; - - // 设置消息头 - 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.open("/dev/tor2", err2); + if(err2){ + std::cout << "Error: " << err2 << std::endl; + std::cout << "请检查您的串口/dev/ttyUSB0 是否已经准备好：\n 1.读写权限是否打开（默认不打开) \n 2.串口名称是否正确" << std::endl; + return ; } - // 预留控制指令 - buf[3 + length - 1] = ctrlFlag; //buf[7] + sp2.set_option(serial_port::baud_rate(115200)); + 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)); + sp2.set_option(serial_port::character_size(8)); - // 设置校验值、消息尾 - 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)); - - - + 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}; - bool succeedReadFlag = false; + unsigned char buf[11]={0}; //========================================================= // 此段代码可以读数据的结尾，进而来进行读取数据的头部 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::cout <<"筛选前："<<" {"<< string2hex(str) << "} " <>1)^0x8C; - else - crc >>= 1; - } - } - return crc; -} +void clearBothSerialBuffers() { + boost::system::error_code ec; + char buffer[256]; + // 清空 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; + } +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h b/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h old mode 100644 new mode 100755 index 7f835be..1a2d5dc --- a/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h +++ b/src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h @@ -8,11 +8,14 @@ #include #include #include -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 write3float(float x, float y, float yaw); -extern bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlFlag); -unsigned char getCrc8(unsigned char *ptr, unsigned short len); - +extern void passballserialInit(); +extern void clearBothSerialBuffers(); +extern bool read5float(bool &dian,bool &passball); +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 diff --git a/src/FAST_LIO_LOCALIZATION/src/position.cpp b/src/FAST_LIO_LOCALIZATION/src/position.cpp new file mode 100755 index 0000000..c98fa56 --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/position.cpp @@ -0,0 +1,130 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +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 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 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(x); + float y_f = static_cast(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(); + } +} diff --git a/src/FAST_LIO_LOCALIZATION/src/preprocess.cpp b/src/FAST_LIO_LOCALIZATION/src/preprocess.cpp old mode 100644 new mode 100755 index d809508..bd86e99 --- a/src/FAST_LIO_LOCALIZATION/src/preprocess.cpp +++ b/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 &pcl_out, deque &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 &pcl_out, + deque &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 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 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 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: "<points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) { - if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) + valid_num ++; + if (valid_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; - 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) + 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, 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 (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_buff[msg->points[i].line].push_back(pl_full[i]); + pl_surf.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 &pl = pl_buff[j]; - plsize = pl.size(); - vector &types = typess[j]; - types.clear(); - types.resize(plsize); - plsize--; - for(uint i=0; ipoints[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00)) - { - valid_num ++; - if (valid_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; - pl_full[i].curvature = msg->points[i].offset_time / float(1000000); //use curvature as time of each laser points - 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_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,93 +369,33 @@ 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) + + + double time_stamp = msg->header.stamp.toSec(); + // cout << "===================================" << endl; + // printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS); + for (int i = 0; i < pl_orig.points.size(); i++) { - for (int i = 0; i < N_SCANS; i++) - { - pl_buff[i].clear(); - pl_buff[i].reserve(plsize); - } + if (i % point_filter_num != 0) continue; - 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; + 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 * 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; + 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; + added_pt.curvature = pl_orig.points[i].t * time_unit_scale; // curvature unit: ms - 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 &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); - for (int i = 0; i < pl_orig.points.size(); i++) - { - if (i % point_filter_num != 0) continue; - - 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; - - pl_surf.points.push_back(added_pt); - } + 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 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 is_first(N_SCANS,true); std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point std::vector 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<<"!!!!!!"< (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 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 is_first(N_SCANS,true); + std::vector yaw_fp(N_SCANS, 0.0); // yaw of first scan point + std::vector yaw_last(N_SCANS, 0.0); // yaw of last scan point + std::vector 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,138 +545,63 @@ 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 < plsize; i++) { - for (int i = 0; i < N_SCANS; i++) - { - pl_buff[i].clear(); - pl_buff[i].reserve(plsize); - } + PointType added_pt; + // cout<<"!!!!!!"<= 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 + int layer = pl_orig.points[i].ring; + double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957; - if (!given_offset_time) + if (is_first[layer]) { - 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; + // 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; } - pl_buff[layer].points.push_back(added_pt); + // 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; } - for (int j = 0; j < N_SCANS; j++) + if (i % point_filter_num == 0 && !std::isnan(added_pt.x) && !std::isnan(added_pt.y) && !std::isnan(added_pt.z)) { - PointCloudXYZI &pl = pl_buff[j]; - int linesize = pl.size(); - if (linesize < 2) continue; - vector &types = typess[j]; - types.clear(); - types.resize(linesize); - linesize--; - for (uint i = 0; i < linesize; i++) + 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)) { - 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; - // cout<<"!!!!!!"< blind) - { - pl_surf.points.push_back(added_pt); - } + pl_surf.points.push_back(added_pt); } } } + } void Preprocess::give_feature(pcl::PointCloud &pl, vector &types) @@ -513,52 +678,6 @@ void Preprocess::give_feature(pcl::PointCloud &pl, vector &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; diff --git a/src/FAST_LIO_LOCALIZATION/src/preprocess.h b/src/FAST_LIO_LOCALIZATION/src/preprocess.h old mode 100644 new mode 100755 index 5f0da48..a5e2e18 --- a/src/FAST_LIO_LOCALIZATION/src/preprocess.h +++ b/src/FAST_LIO_LOCALIZATION/src/preprocess.h @@ -10,11 +10,14 @@ using namespace std; typedef pcl::PointXYZINormal PointType; typedef pcl::PointCloud 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 &pcl_out, deque &time_lidar, const int required_frame_num, int scan_count); + + void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque &pcl_out, deque &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 typess[128]; //maximum 128 line lidar - int lidar_type, point_filter_num, N_SCANS;; - double blind; - double max_scan_range; - bool feature_enabled, given_offset_time; + float time_unit_scale; + int lidar_type, point_filter_num, N_SCANS, SCAN_RATE, time_unit; + double blind, det_range; + 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 &types); void pub_func(PointCloudXYZI &pl, const ros::Time &ct); int plane_judge(const PointCloudXYZI &pl, vector &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct); diff --git a/src/FAST_LIO_LOCALIZATION/src/send.cpp b/src/FAST_LIO_LOCALIZATION/src/send.cpp new file mode 100755 index 0000000..cac00fd --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/send.cpp @@ -0,0 +1,195 @@ +#include +#include +#include +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "send_pose"); + ros::NodeHandle nh; + + // 创建预定义目标点列表 + std::vector 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("/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; +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/test_goal.cpp b/src/FAST_LIO_LOCALIZATION/src/test_goal.cpp new file mode 100755 index 0000000..05ce58c --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/test_goal.cpp @@ -0,0 +1,84 @@ +#include +#include +#include +#include // 关键：添加四元数转换工具 +#include +#include +#include +#include +#include + +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("car", car_frame, "car"); + nh.param("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("/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; +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/tor1_serial.cpp b/src/FAST_LIO_LOCALIZATION/src/tor1_serial.cpp new file mode 100755 index 0000000..70afc21 --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/tor1_serial.cpp @@ -0,0 +1,87 @@ +#include +#include +#include +#include +#include +#include + +static serial::Serial ser; +static std::mutex serial_mutex; + +// 串口参数 +void serialInit() { + std::lock_guard 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 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(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(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("/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."); + } +} +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/uart.cpp b/src/FAST_LIO_LOCALIZATION/src/uart.cpp new file mode 100755 index 0000000..01e9887 --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/uart.cpp @@ -0,0 +1,180 @@ +#include +#include +#include +#include +#include +#include +#include // 文件控制定义 +#include // UNIX标准函数定义 +#include // POSIX终端控制定义 +#include // 字符串操作函数 +#include +#include +#include +#include +#include +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(msg->vx); + float sent_y = static_cast(msg->vy); + float sent_yaw = static_cast(msg->yaw); + float pos = static_cast(msg->pos); + float ang = static_cast(msg->ang); + bool reach = static_cast(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 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; +} \ No newline at end of file diff --git a/src/FAST_LIO_LOCALIZATION/src/underpan_serial.cpp b/src/FAST_LIO_LOCALIZATION/src/underpan_serial.cpp new file mode 100755 index 0000000..09e59be --- /dev/null +++ b/src/FAST_LIO_LOCALIZATION/src/underpan_serial.cpp @@ -0,0 +1,74 @@ +#include +#include +#include +#include +#include +#include + +static serial::Serial ser; +static std::mutex serial_mutex; + +// 串口参数 +void serialInit() { + std::lock_guard 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 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(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("/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(); +} +} \ No newline at end of file diff --git a/src/Livox-SDK2 b/src/Livox-SDK2 new file mode 160000 index 0000000..6a94015 --- /dev/null +++ b/src/Livox-SDK2 @@ -0,0 +1 @@ +Subproject commit 6a940156dd7151c3ab6a52442d86bc83613bd11b diff --git a/src/Point-LIO b/src/Point-LIO new file mode 160000 index 0000000..97b0042 --- /dev/null +++ b/src/Point-LIO @@ -0,0 +1 @@ +Subproject commit 97b0042e397ec71c327966abebff9b4b7e2c154a diff --git a/src/livox_ros_driver2/config/MID360_config.json b/src/livox_ros_driver2/config/MID360_config.json index 2d659c8..b3d2f0f 100644 --- a/src/livox_ros_driver2/config/MID360_config.json +++ b/src/livox_ros_driver2/config/MID360_config.json @@ -24,7 +24,7 @@ } }, "lidar_configs" : [ - { + { "ip" : "192.168.1.176", "pcl_data_type" : 1, "pattern_mode" : 0, @@ -34,7 +34,7 @@ "yaw": 0.0, "x": 0, "y": 0, - "z": 0 + "z": 0.0 } } ] diff --git a/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch b/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch index 1e08745..4fd04e3 100644 --- a/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch +++ b/src/livox_ros_driver2/launch_ROS1/msg_MID360.launch @@ -1,5 +1,4 @@ - @@ -30,11 +29,12 @@ + args="$(arg cmdline_arg)" + output="screen"/> - + - + \ No newline at end of file diff --git a/src/pcd2pgm/launch/run.launch b/src/pcd2pgm/launch/run.launch index 6371265..d841534 100755 --- a/src/pcd2pgm/launch/run.launch +++ b/src/pcd2pgm/launch/run.launch @@ -2,11 +2,11 @@ - +i - + - + diff --git a/src/pcd2pgm/src/pcd2pgm.cpp b/src/pcd2pgm/src/pcd2pgm.cpp index 3ebb0eb..db2c5c1 100644 --- a/src/pcd2pgm/src/pcd2pgm.cpp +++ b/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_max = 2.0; +double thre_z_min = 0.05; +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_max", thre_z_max, 2.0); + private_nh.param("thre_z_min", thre_z_min, 0.05); + 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);