point_lio
This commit is contained in:
parent
de1d9af947
commit
c47a95799e
@ -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(
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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,
|
||||
|
||||
@ -7,13 +7,13 @@
|
||||
|
||||
<param name="feature_extract_enable" type="bool" value="0"/>
|
||||
<param name="point_filter_num" type="int" value="3"/>
|
||||
<param name="max_iteration" type="int" value="3" />
|
||||
<param name="max_iteration" type="int" value="6" />
|
||||
<param name="filter_size_surf" type="double" value="0.5" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="cube_side_length" type="double" value="1000" />
|
||||
<param name="runtime_pos_log_enable" type="bool" value="0" />
|
||||
<param name="runtime_pos_log_enable" type="bool" value="1" />
|
||||
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
|
||||
|
||||
<node pkg="tf" type="static_transform_publisher" name="livox_tf" args="0 0 0 180 0 0 body camera_init 100"/>
|
||||
<group if="$(arg rviz)">
|
||||
<node launch-prefix="nice" pkg="rviz" type="rviz" name="rviz" args="-d $(find fast_lio)/rviz_cfg/loam_livox.rviz" />
|
||||
</group>
|
||||
|
||||
@ -488,6 +488,7 @@ void publish_frame_world(const ros::Publisher & pubLaserCloudFull)
|
||||
{
|
||||
RGBpointBodyToWorld(&laserCloudFullRes->points[i], \
|
||||
&laserCloudWorld->points[i]);
|
||||
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2 laserCloudmsg;
|
||||
@ -538,6 +539,7 @@ void publish_frame_body(const ros::Publisher & pubLaserCloudFull_body)
|
||||
{
|
||||
RGBpointBodyLidarToIMU(&feats_undistort->points[i], \
|
||||
&laserCloudIMUBody->points[i]);
|
||||
|
||||
}
|
||||
|
||||
sensor_msgs::PointCloud2 laserCloudmsg;
|
||||
@ -764,8 +766,8 @@ int main(int argc, char** argv)
|
||||
nh.param<bool>("publish/scan_bodyframe_pub_en",scan_body_pub_en, true);
|
||||
nh.param<int>("max_iteration",NUM_MAX_ITERATIONS,4);
|
||||
nh.param<string>("map_file_path",map_file_path,"");
|
||||
nh.param<string>("common/lid_topic",lid_topic,"/livox/lidar");
|
||||
nh.param<string>("common/imu_topic", imu_topic,"/livox/imu");
|
||||
nh.param<string>("common/lid_topic",lid_topic,"livox/lidar");
|
||||
nh.param<string>("common/imu_topic", imu_topic,"livox/imu");
|
||||
nh.param<bool>("common/time_sync_en", time_sync_en, false);
|
||||
nh.param<double>("common/time_offset_lidar_to_imu", time_diff_lidar_to_imu, 0.0);
|
||||
nh.param<double>("filter_size_corner",filter_size_corner_min,0.5);
|
||||
@ -847,17 +849,17 @@ int main(int argc, char** argv)
|
||||
nh.subscribe(lid_topic, 200000, standard_pcl_cbk);
|
||||
ros::Subscriber sub_imu = nh.subscribe(imu_topic, 200000, imu_cbk);
|
||||
ros::Publisher pubLaserCloudFull = nh.advertise<sensor_msgs::PointCloud2>
|
||||
("/cloud_registered", 100000);
|
||||
("cloud_registered", 100000);
|
||||
ros::Publisher pubLaserCloudFull_body = nh.advertise<sensor_msgs::PointCloud2>
|
||||
("/cloud_registered_body", 100000);
|
||||
("cloud_registered_body", 100000);
|
||||
ros::Publisher pubLaserCloudEffect = nh.advertise<sensor_msgs::PointCloud2>
|
||||
("/cloud_effected", 100000);
|
||||
("cloud_effected", 100000);
|
||||
ros::Publisher pubLaserCloudMap = nh.advertise<sensor_msgs::PointCloud2>
|
||||
("/Laser_map", 100000);
|
||||
("Laser_map", 100000);
|
||||
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry>
|
||||
("/Odometry", 100000);
|
||||
("Odometry", 100000);
|
||||
ros::Publisher pubPath = nh.advertise<nav_msgs::Path>
|
||||
("/path", 100000);
|
||||
("path", 100000);
|
||||
//------------------------------------------------------------------------------------------------------
|
||||
signal(SIGINT, SigHandle);
|
||||
ros::Rate rate(5000);
|
||||
|
||||
0
src/FAST_LIO_LOCALIZATION/.gitignore
vendored
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/.gitignore
vendored
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/.gitmodules
vendored
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/.gitmodules
vendored
Normal file → Executable file
56
src/FAST_LIO_LOCALIZATION/CMakeLists.txt
Normal file → Executable file
56
src/FAST_LIO_LOCALIZATION/CMakeLists.txt
Normal file → Executable file
@ -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})
|
||||
0
src/FAST_LIO_LOCALIZATION/LICENSE
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/LICENSE
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/Log/fast_lio_time_log_analysis.m
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/Log/fast_lio_time_log_analysis.m
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/Log/guide.md
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/Log/guide.md
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/Log/plot.py
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/Log/plot.py
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/PCD/1
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/PCD/1
Normal file → Executable file
BIN
src/FAST_LIO_LOCALIZATION/PCD/map.pgm
Executable file
BIN
src/FAST_LIO_LOCALIZATION/PCD/map.pgm
Executable file
Binary file not shown.
7
src/FAST_LIO_LOCALIZATION/PCD/map.yaml
Executable file
7
src/FAST_LIO_LOCALIZATION/PCD/map.yaml
Executable file
@ -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
|
||||
|
||||
BIN
src/FAST_LIO_LOCALIZATION/PCD/map1.pgm
Normal file → Executable file
BIN
src/FAST_LIO_LOCALIZATION/PCD/map1.pgm
Normal file → Executable file
Binary file not shown.
4
src/FAST_LIO_LOCALIZATION/PCD/map1.yaml
Normal file → Executable file
4
src/FAST_LIO_LOCALIZATION/PCD/map1.yaml
Normal file → Executable file
@ -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
|
||||
|
||||
BIN
src/FAST_LIO_LOCALIZATION/PCD/map_test.pgm
Normal file
BIN
src/FAST_LIO_LOCALIZATION/PCD/map_test.pgm
Normal file
Binary file not shown.
7
src/FAST_LIO_LOCALIZATION/PCD/map_test.yaml
Normal file
7
src/FAST_LIO_LOCALIZATION/PCD/map_test.yaml
Normal file
@ -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
|
||||
|
||||
BIN
src/FAST_LIO_LOCALIZATION/PCD/mapc.pgm
Normal file
BIN
src/FAST_LIO_LOCALIZATION/PCD/mapc.pgm
Normal file
Binary file not shown.
7
src/FAST_LIO_LOCALIZATION/PCD/mapc.yaml
Normal file
7
src/FAST_LIO_LOCALIZATION/PCD/mapc.yaml
Normal file
@ -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
|
||||
|
||||
0
src/FAST_LIO_LOCALIZATION/README.md
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/README.md
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/avia.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/avia.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/horizon.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/horizon.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/mid360.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/mid360.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/ouster64.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/ouster64.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/velodyne.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/config/velodyne.yaml
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/doc/demo.GIF
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/doc/demo.GIF
Normal file → Executable file
|
Before Width: | Height: | Size: 13 MiB After Width: | Height: | Size: 13 MiB |
0
src/FAST_LIO_LOCALIZATION/doc/demo_accu.GIF
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/doc/demo_accu.GIF
Normal file → Executable file
|
Before Width: | Height: | Size: 6.7 MiB After Width: | Height: | Size: 6.7 MiB |
0
src/FAST_LIO_LOCALIZATION/doc/demo_init.GIF
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/doc/demo_init.GIF
Normal file → Executable file
|
Before Width: | Height: | Size: 3.0 MiB After Width: | Height: | Size: 3.0 MiB |
0
src/FAST_LIO_LOCALIZATION/doc/demo_init_2.GIF
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/doc/demo_init_2.GIF
Normal file → Executable file
|
Before Width: | Height: | Size: 3.2 MiB After Width: | Height: | Size: 3.2 MiB |
0
src/FAST_LIO_LOCALIZATION/include/Exp_mat.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/Exp_mat.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/common_lib.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/common_lib.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/README.md
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/README.md
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.cpp
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.cpp
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/ikd-Tree/ikd_Tree.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/matplotlibcpp.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/matplotlibcpp.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/so3_math.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/so3_math.h
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/use-ikfom.hpp
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/include/use-ikfom.hpp
Normal file → Executable file
4
src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch
Normal file → Executable file
4
src/FAST_LIO_LOCALIZATION/launch/PointsCloud2toLaserscan.launch
Normal file → Executable file
@ -5,8 +5,8 @@
|
||||
|
||||
<rosparam>
|
||||
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
|
||||
|
||||
0
src/FAST_LIO_LOCALIZATION/launch/align_pcd_map.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/align_pcd_map.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/gdb_debug_example.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/gdb_debug_example.launch
Normal file → Executable file
19
src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch
Normal file → Executable file
19
src/FAST_LIO_LOCALIZATION/launch/localization_MID360.launch
Normal file → Executable file
@ -1,22 +1,21 @@
|
||||
<launch>
|
||||
<!-- Launch file for Livox AVIA LiDAR -->
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
|
||||
<rosparam command="load" file="$(find fast_lio_localization)/config/mid360.yaml" />
|
||||
|
||||
<param name="feature_extract_enable" type="bool" value="1"/>
|
||||
<param name="point_filter_num" type="int" value="3"/>
|
||||
<param name="max_iteration" type="int" value="3" />
|
||||
<param name="filter_size_surf" type="double" value="0.5" />
|
||||
<param name="filter_size_map" type="double" value="0.5" />
|
||||
<param name="point_filter_num" type="int" value="2"/>
|
||||
<param name="max_iteration" type="int" value="6" />
|
||||
<param name="filter_size_surf" type="double" value="0.3" />
|
||||
<param name="filter_size_map" type="double" value="0.3" />
|
||||
<param name="cube_side_length" type="double" value="1000" />
|
||||
<param name="runtime_pos_log_enable" type="bool" value="0" />
|
||||
<node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" output="screen" />
|
||||
|
||||
<!-- <node pkg="fast_lio" type="fastlio_mapping" name="laserMapping" /> -->
|
||||
<!-- -->
|
||||
|
||||
<!-- <arg name="map" default="/home/rm/ws_sentry/src/FAST_LIO/PCD(305_10-20)/scans.pcd" /> -->
|
||||
<arg name="map" default="$(find fast_lio_localization)/PCD/scans.pcd" />
|
||||
<arg name="map" default="$(find fast_lio)/PCD/scans1.pcd" />
|
||||
<!-- loalization-->
|
||||
<node pkg="fast_lio_localization" type="global_localization.py" name="global_localization" output="screen" />
|
||||
|
||||
@ -38,11 +37,9 @@
|
||||
<!-- load 2d map -->
|
||||
<arg name="2dmap" default="M4.yaml" />
|
||||
|
||||
<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/map1.yaml /map:=prior_map"/>
|
||||
<node name = "map_server" pkg = "map_server" type = "map_server" args="$(find fast_lio_localization)/PCD/map1.yaml /map:=prior_map" />
|
||||
|
||||
<!-- pointscloud2 to laserscans -->
|
||||
<include file="$(find fast_lio_localization)/launch/PointsCloud2toLaserscan.launch">
|
||||
</include>
|
||||
|
||||
|
||||
</launch>
|
||||
|
||||
0
src/FAST_LIO_LOCALIZATION/launch/localization_avia.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/localization_avia.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/localization_horizon.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/localization_horizon.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/localization_ouster64.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/localization_ouster64.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/localization_velodyne.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/localization_velodyne.launch
Normal file → Executable file
@ -1,5 +0,0 @@
|
||||
<launch>
|
||||
<node pkg = "fast_lio_localization" type = "flag" name = "flag" output="screen"/>
|
||||
<node pkg = "fast_lio_localization" type = "controlpid" name = "controlpid" output="screen"/>
|
||||
|
||||
</launch>
|
||||
8
src/FAST_LIO_LOCALIZATION/launch/nav.launch
Normal file → Executable file
8
src/FAST_LIO_LOCALIZATION/launch/nav.launch
Normal file → Executable file
@ -1,5 +1,7 @@
|
||||
<launch>
|
||||
<include file="$(find livox_ros_driver2)/launch_ROS1/msg_MID360.launch"/>
|
||||
<include file="$(find fast_lio_localization)/launch/localization_MID360.launch"/>
|
||||
|
||||
<node pkg="fast_lio_localization" type="flag" name="flag" />
|
||||
<node pkg="fast_lio_localization" type="uart" name="uart" output="screen"/>
|
||||
<node pkg="fast_lio_localization" type="all_move" name="all_move" output="screen"/>
|
||||
<!-- <node pkg="tf2_ros" type="static_transform_publisher" name="map_to_camera_init"
|
||||
args="0 0 0 0 0 0 map camera_init" /> -->
|
||||
</launch>
|
||||
5
src/FAST_LIO_LOCALIZATION/launch/r_serial.launch
Executable file
5
src/FAST_LIO_LOCALIZATION/launch/r_serial.launch
Executable file
@ -0,0 +1,5 @@
|
||||
<launch>
|
||||
<node pkg="fast_lio_localization" type="underpan_serial" name="underpan_serial" output="screen"/>
|
||||
<!-- <node pkg="fast_lio_localization" type="tor1_serial" name="tor1_serial" output="screen"/>
|
||||
<node pkg="fast_lio_localization" type="position" name="position" output="screen"/> -->
|
||||
</launch>
|
||||
2
src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch
Normal file → Executable file
2
src/FAST_LIO_LOCALIZATION/launch/sentry_build_map.launch
Normal file → Executable file
@ -3,7 +3,7 @@
|
||||
|
||||
<arg name="rviz" default="true" />
|
||||
<arg name="use_sim_time" value="true"/>
|
||||
<arg name="map" default="$(find fast_lio)/PCD/scanss.pcd" />
|
||||
<arg name="map" default="$(find fast_lio)/PCD/scans.pcd" />
|
||||
|
||||
<!-- fast_lio -->
|
||||
<rosparam command="load" file="$(find fast_lio)/config/mid360.yaml" />
|
||||
|
||||
0
src/FAST_LIO_LOCALIZATION/launch/sentry_localize.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/sentry_localize.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/sentry_localize_odom.launch
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/launch/sentry_localize_odom.launch
Normal file → Executable file
11
src/FAST_LIO_LOCALIZATION/msg/DataAI.msg
Executable file
11
src/FAST_LIO_LOCALIZATION/msg/DataAI.msg
Executable file
@ -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
|
||||
8
src/FAST_LIO_LOCALIZATION/msg/DataMCU.msg
Executable file
8
src/FAST_LIO_LOCALIZATION/msg/DataMCU.msg
Executable file
@ -0,0 +1,8 @@
|
||||
float32 q0
|
||||
float32 q1
|
||||
float32 q2
|
||||
float32 q3
|
||||
float32 yaw
|
||||
float32 pit
|
||||
float32 rol
|
||||
uint8 notice
|
||||
5
src/FAST_LIO_LOCALIZATION/msg/DataNav.msg
Executable file
5
src/FAST_LIO_LOCALIZATION/msg/DataNav.msg
Executable file
@ -0,0 +1,5 @@
|
||||
bool reached
|
||||
|
||||
float32 x
|
||||
float32 y
|
||||
float32 yaw
|
||||
3
src/FAST_LIO_LOCALIZATION/msg/DataRef.msg
Executable file
3
src/FAST_LIO_LOCALIZATION/msg/DataRef.msg
Executable file
@ -0,0 +1,3 @@
|
||||
uint16 remain_hp
|
||||
uint8 game_progress
|
||||
uint16 stage_remain_time
|
||||
8
src/FAST_LIO_LOCALIZATION/msg/GoalPose.msg
Executable file
8
src/FAST_LIO_LOCALIZATION/msg/GoalPose.msg
Executable file
@ -0,0 +1,8 @@
|
||||
float32 x
|
||||
float32 y
|
||||
float32 angle
|
||||
|
||||
float32 max_speed
|
||||
float32 tolerance
|
||||
float32 circle
|
||||
bool rotor
|
||||
0
src/FAST_LIO_LOCALIZATION/msg/Pose6D.msg
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/msg/Pose6D.msg
Normal file → Executable file
20
src/FAST_LIO_LOCALIZATION/msg/Ps2Data.msg
Executable file
20
src/FAST_LIO_LOCALIZATION/msg/Ps2Data.msg
Executable file
@ -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
|
||||
|
||||
3
src/FAST_LIO_LOCALIZATION/msg/position.msg
Executable file
3
src/FAST_LIO_LOCALIZATION/msg/position.msg
Executable file
@ -0,0 +1,3 @@
|
||||
int8 passball
|
||||
float32 x
|
||||
float32 y
|
||||
1
src/FAST_LIO_LOCALIZATION/msg/serial.msg
Executable file
1
src/FAST_LIO_LOCALIZATION/msg/serial.msg
Executable file
@ -0,0 +1 @@
|
||||
uint8[] data # 定义二进制数据字段
|
||||
2
src/FAST_LIO_LOCALIZATION/msg/underpan.msg
Executable file
2
src/FAST_LIO_LOCALIZATION/msg/underpan.msg
Executable file
@ -0,0 +1,2 @@
|
||||
int8 dian
|
||||
int8 passball
|
||||
4
src/FAST_LIO_LOCALIZATION/package.xml
Normal file → Executable file
4
src/FAST_LIO_LOCALIZATION/package.xml
Normal file → Executable file
@ -26,6 +26,7 @@
|
||||
<build_depend>tf</build_depend>
|
||||
<build_depend>pcl_ros</build_depend>
|
||||
<build_depend>livox_ros_driver2</build_depend>
|
||||
<build_depend>serial</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
|
||||
<run_depend>geometry_msgs</run_depend>
|
||||
@ -37,11 +38,12 @@
|
||||
<run_depend>tf</run_depend>
|
||||
<run_depend>pcl_ros</run_depend>
|
||||
<run_depend>livox_ros_driver2</run_depend>
|
||||
<run_depend>serial</run_depend>
|
||||
<run_depend>message_runtime</run_depend>
|
||||
|
||||
<test_depend>rostest</test_depend>
|
||||
<test_depend>rosbag</test_depend>
|
||||
|
||||
|
||||
<export>
|
||||
</export>
|
||||
</package>
|
||||
|
||||
0
src/FAST_LIO_LOCALIZATION/rviz_cfg/.gitignore
vendored
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/rviz_cfg/.gitignore
vendored
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/rviz_cfg/loam_livox.rviz
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/rviz_cfg/loam_livox.rviz
Normal file → Executable file
31
src/FAST_LIO_LOCALIZATION/rviz_cfg/localization.rviz
Normal file → Executable file
31
src/FAST_LIO_LOCALIZATION/rviz_cfg/localization.rviz
Normal file → Executable file
@ -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
|
||||
|
||||
9
src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_build_map.rviz
Normal file → Executable file
9
src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_build_map.rviz
Normal file → Executable file
@ -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
|
||||
|
||||
0
src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_localize.rviz
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/rviz_cfg/sentry_localize.rviz
Normal file → Executable file
@ -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()
|
||||
@ -1,4 +1,4 @@
|
||||
#!/usr/bin/python3
|
||||
#!/usr/bin/python3cmd_pid_publishe
|
||||
# coding=utf8
|
||||
from __future__ import print_function, division, absolute_import
|
||||
|
||||
|
||||
23
src/FAST_LIO_LOCALIZATION/src/.vscode/c_cpp_properties.json
vendored
Executable file
23
src/FAST_LIO_LOCALIZATION/src/.vscode/c_cpp_properties.json
vendored
Executable file
@ -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
|
||||
}
|
||||
10
src/FAST_LIO_LOCALIZATION/src/.vscode/settings.json
vendored
Executable file
10
src/FAST_LIO_LOCALIZATION/src/.vscode/settings.json
vendored
Executable file
@ -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"
|
||||
]
|
||||
}
|
||||
136
src/FAST_LIO_LOCALIZATION/src/IMU_Processing.cpp
Executable file
136
src/FAST_LIO_LOCALIZATION/src/IMU_Processing.cpp
Executable file
@ -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;
|
||||
}
|
||||
}
|
||||
59
src/FAST_LIO_LOCALIZATION/src/IMU_Processing.h
Executable file
59
src/FAST_LIO_LOCALIZATION/src/IMU_Processing.h
Executable file
@ -0,0 +1,59 @@
|
||||
#pragma once
|
||||
#include <cmath>
|
||||
#include <math.h>
|
||||
// #include <deque>
|
||||
// #include <mutex>
|
||||
// #include <thread>
|
||||
#include <csignal>
|
||||
#include <ros/ros.h>
|
||||
// #include <so3_math.h>
|
||||
#include <Eigen/Eigen>
|
||||
// #include "Estimator.h"
|
||||
#include <common_lib.h>
|
||||
#include <pcl/common/io.h>
|
||||
#include <pcl/point_cloud.h>
|
||||
#include <pcl/point_types.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <pcl/kdtree/kdtree_flann.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <eigen_conversions/eigen_msg.h>
|
||||
#include <pcl_conversions/pcl_conversions.h>
|
||||
#include <sensor_msgs/PointCloud2.h>
|
||||
|
||||
/// *************Preconfiguration
|
||||
|
||||
#define MAX_INI_COUNT (100)
|
||||
const bool time_list(PointType &x, PointType &y); // {return (x.curvature < y.curvature);};
|
||||
|
||||
/// *************IMU Process and undistortion
|
||||
class ImuProcess
|
||||
{
|
||||
public:
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
|
||||
ImuProcess();
|
||||
~ImuProcess();
|
||||
|
||||
void Reset();
|
||||
void Process(const MeasureGroup &meas, PointCloudXYZI::Ptr pcl_un_);
|
||||
void set_gyr_cov(const V3D &scaler);
|
||||
void set_acc_cov(const V3D &scaler);
|
||||
void Set_init(Eigen::Vector3d &tmp_gravity, Eigen::Matrix3d &rot);
|
||||
|
||||
MD(12, 12) state_cov = MD(12, 12)::Identity();
|
||||
int lidar_type;
|
||||
V3D gravity_;
|
||||
bool imu_en;
|
||||
V3D mean_acc;
|
||||
bool imu_need_init_ = true;
|
||||
bool after_imu_init_ = false;
|
||||
bool b_first_frame_ = true;
|
||||
double time_last_scan = 0.0;
|
||||
V3D cov_gyr_scale = V3D(0.0001, 0.0001, 0.0001);
|
||||
V3D cov_vel_scale = V3D(0.0001, 0.0001, 0.0001);
|
||||
|
||||
private:
|
||||
void IMU_init(const MeasureGroup &meas, int &N);
|
||||
V3D mean_gyr;
|
||||
int init_iter_num = 1;
|
||||
};
|
||||
0
src/FAST_LIO_LOCALIZATION/src/IMU_Processing.hpp
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/src/IMU_Processing.hpp
Normal file → Executable file
274
src/FAST_LIO_LOCALIZATION/src/all_move.cpp
Executable file
274
src/FAST_LIO_LOCALIZATION/src/all_move.cpp
Executable file
@ -0,0 +1,274 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <geometry_msgs/Quaternion.h>
|
||||
#include <fast_lio_localization/GoalPose.h>
|
||||
#include <fast_lio_localization/DataAI.h>
|
||||
#include <fast_lio_localization/DataNav.h>
|
||||
#include <atomic>
|
||||
#include <thread>
|
||||
#include <cmath>
|
||||
|
||||
namespace rm_simpal_move {
|
||||
|
||||
struct AHRS_Eulr_t {
|
||||
float yaw;
|
||||
float rol;
|
||||
float pit;
|
||||
};
|
||||
|
||||
struct Goal_t {
|
||||
float x;
|
||||
float y;
|
||||
float angle;
|
||||
float range;
|
||||
float max_speed;
|
||||
float tolerance;
|
||||
bool rotor;
|
||||
};
|
||||
|
||||
class RMSimpleMove {
|
||||
public:
|
||||
RMSimpleMove(ros::NodeHandle nh);
|
||||
~RMSimpleMove();
|
||||
|
||||
private:
|
||||
void timer_callback(const ros::TimerEvent&);
|
||||
void goal_pose_callback(const fast_lio_localization::GoalPoseConstPtr& msg);
|
||||
void reach_location_callback(const std_msgs::Bool::ConstPtr& msg);
|
||||
int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const geometry_msgs::Quaternion &quat);
|
||||
float calc_linear_velocity(float distance, float max_speed);
|
||||
float calc_angular_velocity(float yaw, float target_yaw);
|
||||
bool is_goal_reached(float x, float y, float tolerance);
|
||||
fast_lio_localization::DataAI data_ai_msg;
|
||||
|
||||
ros::NodeHandle nh_;
|
||||
ros::NodeHandle private_nh_;
|
||||
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||
std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
|
||||
ros::Timer timer_;
|
||||
ros::Subscriber goal_pose_sub_;
|
||||
ros::Subscriber reach_location_sub_;
|
||||
ros::Publisher data_ai_pub_;
|
||||
ros::Publisher data_nav_pub_;
|
||||
std::atomic<bool> running_;
|
||||
std::atomic<bool> goal_reached_;
|
||||
AHRS_Eulr_t current_eulr_;
|
||||
Goal_t goal_pose_;
|
||||
float distant;
|
||||
float angle;
|
||||
};
|
||||
|
||||
RMSimpleMove::RMSimpleMove(ros::NodeHandle nh)
|
||||
: nh_(nh), private_nh_("~"), running_(true), goal_reached_(false)
|
||||
{
|
||||
tf_buffer_.reset(new tf2_ros::Buffer());
|
||||
tf_listener_.reset(new tf2_ros::TransformListener(*tf_buffer_));
|
||||
tf_broadcaster_.reset(new tf2_ros::TransformBroadcaster());
|
||||
|
||||
timer_ = nh_.createTimer(ros::Duration(0.01), &RMSimpleMove::timer_callback, this);
|
||||
goal_pose_sub_ = nh_.subscribe("/goal_pose", 10, &RMSimpleMove::goal_pose_callback, this);
|
||||
reach_location_sub_ = nh_.subscribe("/localization_ok", 10, &RMSimpleMove::reach_location_callback,this);
|
||||
data_ai_pub_ = nh_.advertise<fast_lio_localization::DataAI>("/chassis/data_ai", 10);
|
||||
data_nav_pub_ = nh_.advertise<fast_lio_localization::DataNav>("/chassis/data_nav", 10);
|
||||
|
||||
ROS_INFO("RMSimpleMove 启动！！！");
|
||||
}
|
||||
|
||||
RMSimpleMove::~RMSimpleMove()
|
||||
{
|
||||
running_.store(false);
|
||||
}
|
||||
void RMSimpleMove::timer_callback(const ros::TimerEvent&)
|
||||
{
|
||||
try
|
||||
{
|
||||
geometry_msgs::TransformStamped trans;
|
||||
trans = tf_buffer_->lookupTransform("bash", "car", ros::Time(0), ros::Duration(0.01));
|
||||
geometry_msgs::TransformStamped maps;
|
||||
maps = tf_buffer_->lookupTransform("map", "car", ros::Time(0), ros::Duration(0.01));
|
||||
AHRS_GetEulr(¤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;
|
||||
}
|
||||
226
src/FAST_LIO_LOCALIZATION/src/chong.cpp
Executable file
226
src/FAST_LIO_LOCALIZATION/src/chong.cpp
Executable file
@ -0,0 +1,226 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <fast_lio_localization/GoalPose.h>
|
||||
#include <fast_lio_localization/DataAI.h>
|
||||
#include <fast_lio_localization/DataNav.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <Eigen/Core> // 核心矩阵运算
|
||||
#include <Eigen/Geometry> // 几何变换相关
|
||||
#include "mbot_linux_serial.h"
|
||||
struct Pose2D {
|
||||
double x;
|
||||
double y;
|
||||
double yaw;
|
||||
};
|
||||
class obtain_tf {
|
||||
public:
|
||||
obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
|
||||
fetchTransform(); // 在构造函数中调用 fetchTransform 方法
|
||||
}
|
||||
double x, y, yaw;
|
||||
double ob_x() {
|
||||
return x;
|
||||
}
|
||||
double ob_y() {
|
||||
return y;
|
||||
}
|
||||
double ob_yaw() {
|
||||
return yaw;
|
||||
}
|
||||
bool fetchTransform() {
|
||||
//ROS_INFO("等待变换: %s -> %s", father_frame_.c_str(), child_frame_.c_str());
|
||||
try {
|
||||
transform_ = tf_buffer_.lookupTransform(
|
||||
father_frame_,
|
||||
child_frame_,
|
||||
ros::Time(0),
|
||||
ros::Duration(10.0)
|
||||
);
|
||||
//ROS_INFO("变换获取成功");
|
||||
x = transform_.transform.translation.x;
|
||||
y = transform_.transform.translation.y;
|
||||
yaw = suanyaw();
|
||||
return true;
|
||||
} catch (tf2::TransformException &ex) {
|
||||
// ROS_ERROR("变换获取失败: %s", ex.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
private:
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
std::string father_frame_;
|
||||
std::string child_frame_;
|
||||
geometry_msgs::TransformStamped transform_;
|
||||
|
||||
|
||||
double suanyaw() {
|
||||
tf2::Quaternion q(
|
||||
transform_.transform.rotation.x,
|
||||
transform_.transform.rotation.y,
|
||||
transform_.transform.rotation.z,
|
||||
transform_.transform.rotation.w
|
||||
);
|
||||
double roll, pitch, yaw;
|
||||
tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
|
||||
return yaw;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
class map_to_odom {
|
||||
public:
|
||||
map_to_odom(ros::NodeHandle nh,Pose2D pose,ros::Publisher send_odom) :nh_(nh),map_car("map","car"),map_odom("map","camera_init"),
|
||||
pose_(pose),send_odom_(send_odom){
|
||||
|
||||
updatePose(); // 在构造函数中调用 updatePose 方法
|
||||
}
|
||||
void send_fix() {
|
||||
nav_msgs::Odometry fix;
|
||||
fix.child_frame_id = "camera_init";
|
||||
fix.header.frame_id = "map";
|
||||
fix.header.stamp = ros::Time::now();
|
||||
fix.pose.pose.position.x = send_x;
|
||||
fix.pose.pose.position.y = send_y;
|
||||
fix.pose.pose.position.z = 0;
|
||||
fix.pose.pose.orientation = q;
|
||||
send_odom_.publish(fix);
|
||||
ROS_INFO("fix:x=%.6f,y=%.6f,yaw=%.6f", fix.pose.pose.position.x, fix.pose.pose.position.y,yaw_);
|
||||
}
|
||||
void run() {
|
||||
ros::Rate rate(10);// 10 Hz
|
||||
int i=0;
|
||||
while (ros::ok()) {
|
||||
i++;
|
||||
updatePose();
|
||||
send_fix();
|
||||
if(i>4)
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
double suan_dx(){
|
||||
return pose2_.x - map_car.ob_x();
|
||||
}
|
||||
double suan_dy(){
|
||||
return pose2_.y - map_car.ob_y();
|
||||
}
|
||||
double suan_dyaw(){
|
||||
return pose2_.yaw - map_car.ob_yaw();
|
||||
}
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::Publisher send_odom_;
|
||||
obtain_tf map_car,map_odom;
|
||||
double send_x, send_y, send_yaw, dis , x_, y_, yaw_,targe_x,targe_y,targe_yaw;
|
||||
geometry_msgs::Quaternion q;
|
||||
Eigen::Vector3f translation; // 平移分量
|
||||
Eigen::Matrix4f T_translation;
|
||||
Eigen::Matrix3f R;
|
||||
Eigen::Matrix3f rot_matrix;
|
||||
Pose2D pose_,pose2_;
|
||||
|
||||
Pose2D transformPose(const Pose2D& sys2_in_sys1) {
|
||||
// 从输入中获取sys2在sys1下的位姿
|
||||
double x = sys2_in_sys1.x;
|
||||
double y = sys2_in_sys1.y;
|
||||
double yaw = sys2_in_sys1.yaw;
|
||||
|
||||
// 计算sys1在sys2下的位姿
|
||||
Pose2D sys1_in_sys2;
|
||||
|
||||
// 通过旋转和平移的逆变换计算新坐标
|
||||
sys1_in_sys2.x = -x * cos(yaw) - y * sin(yaw);
|
||||
sys1_in_sys2.y = x * sin(yaw) - y * cos(yaw);
|
||||
|
||||
// 偏航角取反（角度标准化到 [-π, π]）
|
||||
sys1_in_sys2.yaw = -yaw;
|
||||
|
||||
// 标准化角度
|
||||
while (sys1_in_sys2.yaw > M_PI) sys1_in_sys2.yaw -= 2 * M_PI;
|
||||
while (sys1_in_sys2.yaw < -M_PI) sys1_in_sys2.yaw += 2 * M_PI;
|
||||
|
||||
return sys1_in_sys2;
|
||||
}
|
||||
|
||||
void updatePose() {
|
||||
pose2_ = transformPose(pose_);
|
||||
pose2_.y -= 0.376; // 偏移量
|
||||
pose2_.x += 0.06; // 偏移量
|
||||
x_ = suan_dx() + map_odom.ob_x() ;
|
||||
y_ = suan_dy() + map_odom.ob_y();
|
||||
yaw_ = suan_dyaw() + map_odom.ob_yaw();
|
||||
translation = {x_,y_,0};
|
||||
T_translation = Eigen::Matrix4f::Identity();
|
||||
T_translation.block<3,1>(0,3) = translation;
|
||||
R = Eigen::AngleAxisf(yaw_, Eigen::Vector3f::UnitZ());
|
||||
Eigen::Matrix4f T_rotation = Eigen::Matrix4f::Identity();
|
||||
T_rotation.block<3,3>(0,0) = R;
|
||||
Eigen::Matrix4f T_map_to_odom = T_translation * T_rotation; // 先旋转后平移
|
||||
send_x = T_map_to_odom(0, 3);
|
||||
send_y = T_map_to_odom(1, 3);
|
||||
rot_matrix = T_map_to_odom.block<3, 3>(0, 0);
|
||||
Eigen::Quaternionf quat(rot_matrix);
|
||||
q.w = quat.w();
|
||||
q.x = quat.x();
|
||||
q.y = quat.y();
|
||||
q.z = quat.z();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
geometry_msgs::Quaternion getQuaternionFromYaw(double yaw) {
|
||||
tf2::Quaternion tf_quat;
|
||||
tf_quat.setRPY(0, 0, yaw);
|
||||
geometry_msgs::Quaternion msg_quat;
|
||||
tf2::convert(tf_quat, msg_quat);
|
||||
return msg_quat;
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
ros::init(argc, argv, "fix");
|
||||
ros::NodeHandle nh;
|
||||
// serialInit();
|
||||
ros::Publisher send_odom = nh.advertise<nav_msgs::Odometry>("/map_to_odom",1000);
|
||||
double sick1,sick2,sick3;
|
||||
bool dian,passball;
|
||||
bool sign = true;
|
||||
Pose2D pose;
|
||||
pose.x = 6.67;
|
||||
pose.y = 1.66;
|
||||
pose.yaw = 0.032;
|
||||
// dian=0;
|
||||
// x=0;
|
||||
// y=0;
|
||||
ros::Rate r(100);
|
||||
|
||||
while(ros::ok()){
|
||||
// read5float(dian,passball);
|
||||
dian =1;
|
||||
if(dian==1 || sign){
|
||||
{
|
||||
map_to_odom map(nh,pose,send_odom);
|
||||
map.run();
|
||||
if(abs(map.suan_dx())>0.006 || abs(map.suan_dy())>0.006 || abs(map.suan_dyaw() )>0.01)
|
||||
sign = true;
|
||||
else
|
||||
sign = false;
|
||||
ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
|
||||
}
|
||||
}else{
|
||||
ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
|
||||
}
|
||||
|
||||
r.sleep();
|
||||
}
|
||||
}
|
||||
230
src/FAST_LIO_LOCALIZATION/src/chong0.cpp
Executable file
230
src/FAST_LIO_LOCALIZATION/src/chong0.cpp
Executable file
@ -0,0 +1,230 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <fast_lio_localization/GoalPose.h>
|
||||
#include <fast_lio_localization/DataAI.h>
|
||||
#include <fast_lio_localization/DataNav.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <Eigen/Core> // 核心矩阵运算
|
||||
#include <Eigen/Geometry> // 几何变换相关
|
||||
#include "mbot_linux_serial.h"
|
||||
|
||||
struct Pose2D {
|
||||
double x;
|
||||
double y;
|
||||
double yaw;
|
||||
};
|
||||
|
||||
class obtain_tf {
|
||||
public:
|
||||
obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
|
||||
fetchTransform(); // 在构造函数中调用 fetchTransform 方法
|
||||
}
|
||||
double x, y, yaw;
|
||||
double ob_x() {
|
||||
return x;
|
||||
}
|
||||
double ob_y() {
|
||||
return y;
|
||||
}
|
||||
double ob_yaw() {
|
||||
return yaw;
|
||||
}
|
||||
bool fetchTransform() {
|
||||
if (!tf_buffer_.canTransform(father_frame_, child_frame_, ros::Time(0), ros::Duration(1.0))) {
|
||||
ROS_WARN("等待变换: %s -> %s 不可用", father_frame_.c_str(), child_frame_.c_str());
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
transform_ = tf_buffer_.lookupTransform(
|
||||
father_frame_,
|
||||
child_frame_,
|
||||
ros::Time(0)
|
||||
);
|
||||
x = transform_.transform.translation.x;
|
||||
y = transform_.transform.translation.y;
|
||||
yaw = suanyaw();
|
||||
return true;
|
||||
} catch (tf2::TransformException &ex) {
|
||||
ROS_ERROR("变换获取失败: %s", ex.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
std::string father_frame_;
|
||||
std::string child_frame_;
|
||||
geometry_msgs::TransformStamped transform_;
|
||||
|
||||
|
||||
double suanyaw() {
|
||||
tf2::Quaternion q(
|
||||
transform_.transform.rotation.x,
|
||||
transform_.transform.rotation.y,
|
||||
transform_.transform.rotation.z,
|
||||
transform_.transform.rotation.w
|
||||
);
|
||||
double roll, pitch, yaw;
|
||||
tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
|
||||
return yaw;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
class map_to_odom {
|
||||
public:
|
||||
map_to_odom(ros::NodeHandle nh,Pose2D pose,ros::Publisher send_odom) :nh_(nh),map_car("map","car"),
|
||||
map_odom("map","camera_init"),send_odom_(send_odom),pose_(pose) {
|
||||
|
||||
updatePose(); // 在构造函数中调用 updatePose 方法
|
||||
}
|
||||
void send_fix() {
|
||||
nav_msgs::Odometry fix;
|
||||
fix.child_frame_id = "camera_init";
|
||||
fix.header.frame_id = "map";
|
||||
fix.header.stamp = ros::Time::now();
|
||||
fix.pose.pose.position.x = send_x;
|
||||
fix.pose.pose.position.y = send_y;
|
||||
fix.pose.pose.position.z = 0;
|
||||
fix.pose.pose.orientation = q;
|
||||
send_odom_.publish(fix);
|
||||
ROS_INFO("fix:x=%.6f,y=%.6f,yaw=%.6f", fix.pose.pose.position.x, fix.pose.pose.position.y,yaw_);
|
||||
}
|
||||
void run() {
|
||||
int i=0;
|
||||
while (ros::ok()) {
|
||||
i++;
|
||||
updatePose();
|
||||
send_fix();
|
||||
if(i==10)
|
||||
break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
double suan_dx(){
|
||||
return pose2_.x - map_car.ob_x();
|
||||
}
|
||||
double suan_dy(){
|
||||
return pose2_.y - map_car.ob_y();
|
||||
}
|
||||
double suan_dyaw(){
|
||||
return pose2_.yaw - map_car.ob_yaw();
|
||||
}
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::Publisher send_odom_;
|
||||
obtain_tf map_car,map_odom;
|
||||
double send_x, send_y, send_yaw, dis , x_, y_, yaw_;
|
||||
geometry_msgs::Quaternion q;
|
||||
Eigen::Vector3f translation; // 平移分量
|
||||
Eigen::Matrix4f T_translation;
|
||||
Eigen::Matrix3f R;
|
||||
Eigen::Matrix3f rot_matrix;
|
||||
Pose2D pose_,pose2_;
|
||||
|
||||
Pose2D transformPose(const Pose2D& sys2_in_sys1) {
|
||||
// 从输入中获取sys2在sys1下的位姿
|
||||
double x = sys2_in_sys1.x;
|
||||
double y = sys2_in_sys1.y;
|
||||
double yaw = sys2_in_sys1.yaw;
|
||||
|
||||
// 计算sys1在sys2下的位姿
|
||||
Pose2D sys1_in_sys2;
|
||||
|
||||
// 通过旋转和平移的逆变换计算新坐标
|
||||
sys1_in_sys2.x = -x * cos(yaw) - y * sin(yaw);
|
||||
sys1_in_sys2.y = x * sin(yaw) - y * cos(yaw);
|
||||
|
||||
// 偏航角取反（角度标准化到 [-π, π]）
|
||||
sys1_in_sys2.yaw = -yaw;
|
||||
|
||||
// 标准化角度
|
||||
while (sys1_in_sys2.yaw > M_PI) sys1_in_sys2.yaw -= 2 * M_PI;
|
||||
while (sys1_in_sys2.yaw < -M_PI) sys1_in_sys2.yaw += 2 * M_PI;
|
||||
|
||||
return sys1_in_sys2;
|
||||
}
|
||||
|
||||
|
||||
void updatePose() {
|
||||
pose2_ = transformPose(pose_);
|
||||
pose2_.y -= 0.376; // 偏移量
|
||||
pose2_.x += 0.06; // 偏移量
|
||||
x_ = suan_dx() + map_odom.ob_x() ;
|
||||
y_ = suan_dy() + map_odom.ob_y();
|
||||
yaw_ = suan_dyaw() + map_odom.ob_yaw();
|
||||
translation = {x_,y_,0};
|
||||
T_translation = Eigen::Matrix4f::Identity();
|
||||
T_translation.block<3,1>(0,3) = translation;
|
||||
R = Eigen::AngleAxisf(yaw_, Eigen::Vector3f::UnitZ());
|
||||
Eigen::Matrix4f T_rotation = Eigen::Matrix4f::Identity();
|
||||
T_rotation.block<3,3>(0,0) = R;
|
||||
Eigen::Matrix4f T_map_to_odom = T_translation * T_rotation; // 先旋转后平移
|
||||
send_x = T_map_to_odom(0, 3);
|
||||
send_y = T_map_to_odom(1, 3);
|
||||
rot_matrix = T_map_to_odom.block<3, 3>(0, 0);
|
||||
Eigen::Quaternionf quat(rot_matrix);
|
||||
q.w = quat.w();
|
||||
q.x = quat.x();
|
||||
q.y = quat.y();
|
||||
q.z = quat.z();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
geometry_msgs::Quaternion getQuaternionFromYaw(double yaw) {
|
||||
tf2::Quaternion tf_quat;
|
||||
tf_quat.setRPY(0, 0, yaw);
|
||||
geometry_msgs::Quaternion msg_quat;
|
||||
tf2::convert(tf_quat, msg_quat);
|
||||
return msg_quat;
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
ros::init(argc, argv, "fix");
|
||||
ros::NodeHandle nh;
|
||||
//serialInit();
|
||||
double sick1,sick2,sick3,dian;
|
||||
bool passball;
|
||||
Pose2D pose;
|
||||
bool sign = false;
|
||||
// x=-6;
|
||||
// y=1.5;
|
||||
// pose.x=0;
|
||||
// pose.y=0;
|
||||
pose.x=6.0;
|
||||
pose.y=2.0;
|
||||
pose.yaw=0.00;
|
||||
ros::Publisher send_odom = nh.advertise<nav_msgs::Odometry>("/map_to_odom", 10);
|
||||
ros::Rate rate(20);
|
||||
//clearSerialBuffer();
|
||||
dian = 1;
|
||||
while(ros::ok()){
|
||||
//read4float(dian);
|
||||
|
||||
if(dian==1){
|
||||
{
|
||||
map_to_odom map(nh,pose,send_odom);
|
||||
map.run();
|
||||
if(abs(map.suan_dx())>0.006 || abs(map.suan_dy())>0.006 || abs(map.suan_dyaw() )>0.01)
|
||||
sign = true;
|
||||
else
|
||||
sign = false;
|
||||
ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
|
||||
}
|
||||
}else{
|
||||
ROS_INFO("dian=%.1f,sick1=%.2f,sick2=%.2f,sick3=%.2f",dian,sick1,sick2,sick3);
|
||||
}
|
||||
rate.sleep();
|
||||
}
|
||||
}
|
||||
225
src/FAST_LIO_LOCALIZATION/src/commands.cpp
Executable file
225
src/FAST_LIO_LOCALIZATION/src/commands.cpp
Executable file
@ -0,0 +1,225 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <fast_lio_localization/GoalPose.h>
|
||||
#include <fast_lio_localization/DataAI.h>
|
||||
#include <fast_lio_localization/DataNav.h>
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <Eigen/Core> // 核心矩阵运算
|
||||
#include <Eigen/Geometry> // 几何变换相关
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <serial/serial.h>
|
||||
#include <ros/time.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <boost/asio.hpp>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <fast_lio_localization/underpan.h>
|
||||
#include <fast_lio_localization/position.h>
|
||||
struct Pose2D {
|
||||
double x;
|
||||
double y;
|
||||
double yaw;
|
||||
};
|
||||
|
||||
class obtain_tf {
|
||||
public:
|
||||
obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
|
||||
fetchTransform(); // 在构造函数中调用 fetchTransform 方法
|
||||
}
|
||||
double x, y, yaw;
|
||||
double ob_x() {
|
||||
return x;
|
||||
}
|
||||
double ob_y() {
|
||||
return y;
|
||||
}
|
||||
double ob_yaw() {
|
||||
return yaw;
|
||||
}
|
||||
bool fetchTransform() {
|
||||
if (!tf_buffer_.canTransform(father_frame_, child_frame_, ros::Time(0), ros::Duration(1.0))) {
|
||||
ROS_WARN("等待变换: %s -> %s 不可用", father_frame_.c_str(), child_frame_.c_str());
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
transform_ = tf_buffer_.lookupTransform(
|
||||
father_frame_,
|
||||
child_frame_,
|
||||
ros::Time(0)
|
||||
);
|
||||
x = transform_.transform.translation.x;
|
||||
y = transform_.transform.translation.y;
|
||||
yaw = suanyaw();
|
||||
return true;
|
||||
} catch (tf2::TransformException &ex) {
|
||||
ROS_ERROR("变换获取失败: %s", ex.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
std::string father_frame_;
|
||||
std::string child_frame_;
|
||||
geometry_msgs::TransformStamped transform_;
|
||||
|
||||
|
||||
double suanyaw() {
|
||||
tf2::Quaternion q(
|
||||
transform_.transform.rotation.x,
|
||||
transform_.transform.rotation.y,
|
||||
transform_.transform.rotation.z,
|
||||
transform_.transform.rotation.w
|
||||
);
|
||||
double roll, pitch, yaw;
|
||||
tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
|
||||
return yaw;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
|
||||
class send_goal {
|
||||
public:
|
||||
std::vector<fast_lio_localization::GoalPose> pose_list;
|
||||
|
||||
send_goal(ros::NodeHandle nh) : nh_(nh) {
|
||||
pose_list.resize(3);
|
||||
initializeGoalPoses();
|
||||
}
|
||||
void get_r1(float x , float y) {
|
||||
pose_list[1].x = x;
|
||||
pose_list[1].y = y;
|
||||
pose_list[1].angle = 0.0;
|
||||
pose_list[1].max_speed = 6.0;
|
||||
pose_list[1].tolerance = 0.00;
|
||||
pose_list[1].rotor = 0.0;
|
||||
pose_list[1].circle = -1;
|
||||
}
|
||||
void sendGoalPose(const fast_lio_localization::GoalPose& goal_pose) {
|
||||
goal_pub_.publish(goal_pose);
|
||||
geometry_msgs::TransformStamped transform;
|
||||
transform.header.stamp = ros::Time::now();
|
||||
transform.header.frame_id = "map";
|
||||
transform.child_frame_id = "goal_pose";
|
||||
|
||||
transform.transform.translation.x = goal_pose.x;
|
||||
transform.transform.translation.y = goal_pose.y;
|
||||
transform.transform.translation.z = 0.0;
|
||||
|
||||
tf::Quaternion q = tf::createQuaternionFromYaw(goal_pose.angle);
|
||||
transform.transform.rotation.x = q.x();
|
||||
transform.transform.rotation.y = q.y();
|
||||
transform.transform.rotation.z = q.z();
|
||||
transform.transform.rotation.w = q.w();
|
||||
|
||||
// 广播变换
|
||||
tf_broadcaster.sendTransform(transform);
|
||||
}
|
||||
private:
|
||||
ros::NodeHandle nh_;
|
||||
ros::Publisher goal_pub_;
|
||||
tf::TransformBroadcaster tf_broadcaster;
|
||||
|
||||
void initializeGoalPoses() {
|
||||
pose_list[2];
|
||||
goal_pub_ = nh_.advertise<fast_lio_localization::GoalPose>("goal_pose", 1000);
|
||||
// 初始化目标点
|
||||
pose_list[0].x = 0.490;
|
||||
pose_list[0].y = -3.228;
|
||||
pose_list[0].angle = 0.0;
|
||||
pose_list[0].max_speed = 6.0;
|
||||
pose_list[0].tolerance = 0.00;
|
||||
pose_list[0].rotor = 0.0;
|
||||
pose_list[0].circle = -1;
|
||||
}
|
||||
};
|
||||
|
||||
fast_lio_localization::underpan underpan;
|
||||
fast_lio_localization::position sub_position;
|
||||
bool th_ok = false;
|
||||
void sub_underpan_callback(const fast_lio_localization::underpan::ConstPtr& msg) {
|
||||
underpan.dian = msg->dian;
|
||||
}
|
||||
void sub_position_callback(const fast_lio_localization::position::ConstPtr& msg) {
|
||||
sub_position.passball = msg->passball;
|
||||
sub_position.x = msg->x;
|
||||
sub_position.y = msg->y;
|
||||
}
|
||||
void sub_hz_ok_callback(const std_msgs::Bool::ConstPtr& msg) {
|
||||
th_ok = msg->data;
|
||||
}
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
ros::init(argc, argv, "commands");
|
||||
ros::NodeHandle nh;
|
||||
setlocale(LC_ALL,"");
|
||||
double sick1,sick2,sick3;
|
||||
float r1_x,r1_y,carx, cary,before_x, before_y,dx, dy;
|
||||
before_x = 0.0;
|
||||
before_y = 0.0;
|
||||
obtain_tf map_car("map", "car");
|
||||
bool dian = false, passball = false;
|
||||
std_msgs::Bool msg;
|
||||
int i=0;
|
||||
ros::Subscriber sub_underpan = nh.subscribe("/underpan", 10, sub_underpan_callback);
|
||||
ros::Subscriber sub_r1 = nh.subscribe("/position", 10, sub_position_callback);
|
||||
ros::Subscriber sub_hz_ok = nh.subscribe("/global_localization_th_ok",10, sub_hz_ok_callback);
|
||||
ros::Publisher pub_sign = nh.advertise<std_msgs::Bool>("/localization_ok", 10);
|
||||
send_goal goal(nh);
|
||||
fast_lio_localization::GoalPose goal_pose;
|
||||
ros::Rate rate(100);
|
||||
// clearBothSerialBuffers();
|
||||
goal_pose = goal.pose_list[0];
|
||||
goal.sendGoalPose(goal_pose);
|
||||
while (ros::ok()) {
|
||||
ros::spinOnce();
|
||||
dian = underpan.dian;
|
||||
passball = sub_position.passball;
|
||||
if(dian==1 && th_ok) {
|
||||
map_car.fetchTransform();
|
||||
carx = map_car.ob_x();
|
||||
cary = map_car.ob_y();
|
||||
dx = abs(carx - before_x);
|
||||
dy = abs(cary - before_y);
|
||||
ROS_INFO("dx=%.2f, dy=%.2f", dx, dy);
|
||||
if(dx < 0.02 && dy < 0.02) {
|
||||
i++;
|
||||
if(i > 200) {
|
||||
msg.data = true;
|
||||
pub_sign.publish(msg);
|
||||
}
|
||||
}else{
|
||||
i = 0;
|
||||
}
|
||||
before_x = carx;
|
||||
before_y = cary;
|
||||
}else{
|
||||
i=0;
|
||||
}
|
||||
if(passball) {
|
||||
ROS_INFO("传球模式");
|
||||
// write1float(true,map_car.ob_x(),map_car.ob_y());
|
||||
//read1float(r1_x,r1_y);
|
||||
r1_x = sub_position.x;
|
||||
r1_y = sub_position.y;
|
||||
goal.get_r1(r1_x,r1_y);
|
||||
goal_pose = goal.pose_list[1];
|
||||
goal.sendGoalPose(goal_pose);
|
||||
ROS_INFO("发送目标点: x=%.2f, y=%.2f, angle=%.2f", goal_pose.x, goal_pose.y, goal_pose.angle);
|
||||
}else {
|
||||
// write1float(false,0,0);
|
||||
goal_pose = goal.pose_list[0];
|
||||
goal.sendGoalPose(goal_pose);
|
||||
ROS_INFO("瞄框");
|
||||
}
|
||||
ROS_INFO("dian=%d,passball=%d",dian,passball);
|
||||
rate.sleep();
|
||||
}
|
||||
}
|
||||
@ -1,198 +0,0 @@
|
||||
#include <ros/ros.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <std_msgs/Int64.h>
|
||||
#include <std_msgs/String.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <vector>
|
||||
#include <sensor_msgs/LaserScan.h>
|
||||
#include "mbot_linux_serial.h"
|
||||
class PIDController {
|
||||
public:
|
||||
PIDController(double kp, double ki, double kd)
|
||||
: kp_(kp), ki_(ki), kd_(kd), integral_(0.0), prev_error_(0.0) {}
|
||||
|
||||
|
||||
double control(double setpoint, double pv) {
|
||||
double error = setpoint - pv;
|
||||
integral_ += error;
|
||||
double derivative = error - prev_error_;
|
||||
prev_error_ = error;
|
||||
return kp_ * error + ki_ * integral_ + kd_ * derivative;
|
||||
}//pid
|
||||
|
||||
private:
|
||||
double kp_, ki_, kd_, integral_, prev_error_;
|
||||
};
|
||||
|
||||
struct TargetPose {
|
||||
double x, y, yaw;
|
||||
};
|
||||
|
||||
class PoseController {
|
||||
public:
|
||||
PoseController()
|
||||
: nh_("~"),
|
||||
x_controller_(0.12, 0.0, 0.0),
|
||||
y_controller_(0.15, 0.0, 0.0),
|
||||
yaw_controller_(0.12, 0.0, 0.0),
|
||||
robot_mode_(0) {
|
||||
target_poses_ = {
|
||||
{1.582, 0.00, 0.0},
|
||||
{0.000, 1.60, -M_PI/2},
|
||||
{0.705, 1.60, -M_PI/2},
|
||||
{1.482, 1.60, -M_PI/2},
|
||||
{2.234, 1.60, -M_PI/2},
|
||||
{2.981, 1.60, -M_PI/2},
|
||||
};
|
||||
target_ = target_poses_[0];
|
||||
//laser_sub = nh_.subscribe<sensor_msgs::LaserScan>("/scan", 10, &PoseController::laser_callback, this);
|
||||
pose_subscriber_ = nh_.subscribe("filtered_pose", 10, &PoseController::pose_callback, this);
|
||||
cmd_pid_publisher_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_pid", 10);
|
||||
//position_id_subscriber_ = nh_.subscribe("/position_id", 10, &PoseController::position_id_callback, this);
|
||||
pid_status_publisher_ = nh_.advertise<std_msgs::String>("/pid_status", 10);
|
||||
//robot_mode_subscriber_ = nh_.subscribe("robot_mode", 10, &PoseController::robot_mode_callback, this);
|
||||
goal_subscriber_ = nh_.subscribe("/move_base_simple/goal", 10, &PoseController::goal_callback, this);
|
||||
serial_write3float_=nh_.subscribe("cmd_vel_pid", 10, &PoseController::serial_write3double_callback, this);
|
||||
//scan_sub_=nh_.subscribe("/scan",10,&PoseController::distant,this);
|
||||
}
|
||||
|
||||
private:
|
||||
// void robot_mode_callback(const std_msgs::Int64::ConstPtr& msg) {
|
||||
// robot_mode_ = msg->data;
|
||||
// if (msg->data == 1) {
|
||||
// target_ = target_poses_[0];
|
||||
// }
|
||||
// }
|
||||
|
||||
// void position_id_callback(const std_msgs::Int64::ConstPtr& msg) {
|
||||
// size_t id = static_cast<size_t>(msg->data);
|
||||
// if (id < target_poses_.size()) {
|
||||
// target_ = target_poses_[id];
|
||||
// } else {
|
||||
// ROS_WARN("Invalid position id: %ld", id);
|
||||
// }
|
||||
// }
|
||||
|
||||
void pose_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
|
||||
double x = msg->pose.position.x;
|
||||
double y = msg->pose.position.y;
|
||||
tf2::Quaternion quat;
|
||||
tf2::fromMsg(msg->pose.orientation, quat);
|
||||
tf2::Matrix3x3 m(quat);
|
||||
double roll, pitch, yaw;
|
||||
m.getRPY(roll, pitch, yaw);
|
||||
TargetPose target_local = transformTargetPose(x, y, yaw);//算位姿
|
||||
calculateControl(target_local);//检验
|
||||
}
|
||||
void goal_callback(const geometry_msgs::PoseStamped::ConstPtr& msg) {
|
||||
double x = msg->pose.position.x;
|
||||
double y = msg->pose.position.y;
|
||||
tf2::Quaternion quat;
|
||||
tf2::fromMsg(msg->pose.orientation, quat);
|
||||
tf2::Matrix3x3 m(quat);
|
||||
double roll, pitch, yaw;
|
||||
m.getRPY(roll, pitch, yaw);
|
||||
target_={x,y,yaw};
|
||||
}//目标位置位姿
|
||||
|
||||
TargetPose transformTargetPose(double x, double y, double yaw) {
|
||||
TargetPose target_local;
|
||||
target_local.x = cos(yaw) * (target_.x - x) + sin(yaw) * (target_.y - y);
|
||||
target_local.y = -sin(yaw) * (target_.x - x) + cos(yaw) * (target_.y - y);
|
||||
target_local.yaw = target_.yaw - yaw;
|
||||
if (target_local.yaw > M_PI)
|
||||
target_local.yaw -= 2 * M_PI;
|
||||
else if (target_local.yaw < -M_PI)
|
||||
target_local.yaw += 2 * M_PI;
|
||||
return target_local;
|
||||
}//算位姿
|
||||
|
||||
void calculateControl(const TargetPose &target_local) {
|
||||
double error_threshold = 0.05;
|
||||
double control_x = 0.0, control_y = 0.0, control_yaw = 0.0;
|
||||
if (std::abs(target_local.x) > error_threshold || std::abs(target_local.y) > error_threshold || std::abs(target_local.yaw) > error_threshold) {
|
||||
//校零
|
||||
control_x = x_controller_.control(0.0, target_local.x);
|
||||
control_y = y_controller_.control(0.0, target_local.y);
|
||||
control_yaw = yaw_controller_.control(0.0, target_local.yaw);
|
||||
control_x = std::min(control_x, 0.2);
|
||||
control_y = std::min(control_y, 0.2);//限速
|
||||
} else {
|
||||
std_msgs::String msg;
|
||||
msg.data = (robot_mode_ == 6) ? "OKK" : "OK";
|
||||
pid_status_publisher_.publish(msg);
|
||||
}
|
||||
|
||||
geometry_msgs::Twist cmd;
|
||||
cmd.linear.x = control_x;
|
||||
cmd.linear.y = -control_y;
|
||||
cmd.angular.z = control_yaw;
|
||||
cmd_pid_publisher_.publish(cmd);//发送pid
|
||||
}
|
||||
//发送
|
||||
void serial_write3double_callback(const geometry_msgs::Twist::ConstPtr& msg) {
|
||||
//write3float(msg->linear.x, msg->linear.y, msg->angular.z);
|
||||
write3float2(msg->linear.x, msg->linear.y, 0//msg->angular.z);//lzcdxc
|
||||
ROS_INFO("linear.x: %f, linear.y: %f, angular.z: %f", msg->linear.x, msg->linear.y, msg->angular.z);
|
||||
}
|
||||
|
||||
//避障
|
||||
// void laser_callback(const sensor_msgs::LaserScan::ConstPtr& msg) {
|
||||
// static const float min_distance = 0.45; // 最小安全距离，单位：米
|
||||
// bool obstacle_detected = false;
|
||||
|
||||
// for (size_t i = 0; i < msg->ranges.size(); i=i+30) {
|
||||
// float distance = msg->ranges[i];
|
||||
// if (distance < min_distance && !std::isnan(distance)) {
|
||||
// obstacle_detected = true;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
|
||||
// if (obstacle_detected) {
|
||||
// // 处理避障逻辑
|
||||
// ROS_WARN("stop！！！");
|
||||
// stopRobot();
|
||||
// } else {
|
||||
// // 继续正常运动
|
||||
// continueMoving();
|
||||
// }
|
||||
// }
|
||||
void stopRobot() {
|
||||
geometry_msgs::Twist stop_cmd;
|
||||
stop_cmd.linear.x = 0;
|
||||
stop_cmd.angular.z = 0;
|
||||
cmd_pid_publisher_.publish(stop_cmd);
|
||||
}
|
||||
|
||||
void continueMoving() {
|
||||
// 继续向目标移动的逻辑
|
||||
}
|
||||
ros::NodeHandle nh_;
|
||||
ros::Subscriber pose_subscriber_;
|
||||
ros::Subscriber goal_subscriber_;
|
||||
ros::Publisher cmd_pid_publisher_;
|
||||
ros::Publisher pid_status_publisher_;
|
||||
ros::Subscriber robot_mode_subscriber_;
|
||||
ros::Subscriber position_id_subscriber_;
|
||||
ros::Subscriber serial_write3float_;
|
||||
ros::Subscriber scan_sub_;
|
||||
std::vector<TargetPose> target_poses_;
|
||||
PIDController x_controller_, y_controller_, yaw_controller_;
|
||||
TargetPose target_;
|
||||
ros::Subscriber laser_sub;
|
||||
int robot_mode_;
|
||||
float min_distance = 0.5; // 最小安全距离，单位：米
|
||||
};
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
ros::init(argc, argv, "pose_controller");
|
||||
serialInit();
|
||||
PoseController node;
|
||||
|
||||
ros::spin();
|
||||
return 0;
|
||||
|
||||
}
|
||||
127
src/FAST_LIO_LOCALIZATION/src/flag.cpp
Normal file → Executable file
127
src/FAST_LIO_LOCALIZATION/src/flag.cpp
Normal file → Executable file
@ -1,52 +1,85 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf/transform_listener.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
// tf chage https://zhuanlan.zhihu.com/p/340016739
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
|
||||
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<geometry_msgs::PoseStamped>("/pose_controller/filtered_pose", 10);
|
||||
ros::NodeHandle node;
|
||||
geometry_msgs::PoseStamped pose;
|
||||
tf::TransformListener listener;
|
||||
tf::TransformBroadcaster broadcaster;
|
||||
tf::Transform transform_broadcaster;
|
||||
ros::Duration(1.0).sleep();
|
||||
// 创建静态坐标变换广播器
|
||||
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;
|
||||
};
|
||||
// 设置时间戳和坐标系关系
|
||||
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;
|
||||
}
|
||||
0
src/FAST_LIO_LOCALIZATION/src/laserMapping.cpp
Normal file → Executable file
0
src/FAST_LIO_LOCALIZATION/src/laserMapping.cpp
Normal file → Executable file
361
src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp
Normal file → Executable file
361
src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.cpp
Normal file → Executable file
@ -2,48 +2,31 @@
|
||||
#include <string>
|
||||
#include <unistd.h>
|
||||
|
||||
/*===================================================================
|
||||
程序功能：串口通信测试程序
|
||||
程序编写：公众号：小白学移动机器人
|
||||
其他 ：如果对代码有任何疑问，可以私信小编，一定会回复的。
|
||||
=====================================================================
|
||||
------------------关注公众号，获得更多有趣的分享---------------------
|
||||
===================================================================*/
|
||||
|
||||
|
||||
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) << "} " <<std::endl;
|
||||
// std::cout << "str size = " << str.size() << std::endl;
|
||||
if(str.size() == 12)
|
||||
std::getline(is, str, (char)header_read);
|
||||
if(str.size() == 10)
|
||||
{
|
||||
std::string finalStr(1, header[0]);
|
||||
std::string finalStr(1, header_read);
|
||||
finalStr = finalStr + str;
|
||||
|
||||
// std::cout <<"筛选后："<<" {"<< string2hex(finalStr) << "} " <<std::endl;
|
||||
for (size_t i = 0; i < finalStr.size(); i++)
|
||||
//std::cout << "str size = " << finalStr.size() << std::endl;
|
||||
for (int i = 0; i < 11; i++)
|
||||
{
|
||||
buf[i] = finalStr[i];
|
||||
}
|
||||
succeedReadFlag = true;
|
||||
break;
|
||||
}
|
||||
else
|
||||
@ -225,60 +120,118 @@ bool readSpeed(double &Left_v,double &Right_v,double &Angle,unsigned char &ctrlF
|
||||
//=========================================================
|
||||
|
||||
// 检查信息头
|
||||
if (buf[0]!= header[0] || buf[1] != header[1]) //buf[0] buf[1]
|
||||
{
|
||||
ROS_ERROR("Received message header error!");
|
||||
return false;
|
||||
|
||||
if (buf[1] == usart_idf) {
|
||||
for(int i = 0; i < 4; i++)
|
||||
{ dian_.data[i] = buf[i + 2];
|
||||
passball_.data[i] = buf[i + 6];
|
||||
}
|
||||
// 数据长度
|
||||
length = buf[2]; //buf[2]
|
||||
|
||||
// 检查信息校验值
|
||||
checkSum = getCrc8(buf, 3 + length); //buf[10] 计算得出
|
||||
if (checkSum != buf[3 + length]) //buf[10] 串口接收
|
||||
{
|
||||
ROS_ERROR("Received data check sum error!");
|
||||
return false;
|
||||
}
|
||||
|
||||
// 读取速度值
|
||||
for(int i = 0; i < 2; i++)
|
||||
{
|
||||
leftVelNow.data[i] = buf[i + 3]; //buf[3] buf[4]
|
||||
rightVelNow.data[i] = buf[i + 5]; //buf[5] buf[6]
|
||||
angleNow.data[i] = buf[i + 7]; //buf[7] buf[8]
|
||||
}
|
||||
|
||||
// 读取控制标志位
|
||||
ctrlFlag = buf[9];
|
||||
dian = (bool)dian_.d;
|
||||
passball= (bool)passball_.d; // buf[6]
|
||||
|
||||
Left_v =leftVelNow.d;
|
||||
Right_v =rightVelNow.d;
|
||||
Angle =angleNow.d;
|
||||
|
||||
if((dian==1 || dian==0)&&(passball==1 || passball==0))
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
}
|
||||
bool read1float(float &x, float &y)
|
||||
|
||||
|
||||
{
|
||||
unsigned char checkSum;
|
||||
unsigned char buf[11]={0};
|
||||
//=========================================================
|
||||
// 此段代码可以读数据的结尾，进而来进行读取数据的头部
|
||||
try
|
||||
{
|
||||
boost::asio::streambuf response;
|
||||
boost::asio::read_until(sp2, response,(char)ender_read, err2);
|
||||
|
||||
std::string str;
|
||||
std::istream is(&response);
|
||||
|
||||
while(response.size() != 0)
|
||||
{
|
||||
std::getline(is, str, (char)header_read);
|
||||
if(str.size() == 10)
|
||||
{
|
||||
std::string finalStr(1, header_read);
|
||||
finalStr = finalStr + str;
|
||||
//std::cout << "str size = " << finalStr.size() << std::endl;
|
||||
for (int i = 0; i < 11; i++)
|
||||
{
|
||||
buf[i] = finalStr[i];
|
||||
}
|
||||
break;
|
||||
}
|
||||
else
|
||||
{
|
||||
continue;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch(boost::system::system_error &err2)
|
||||
{
|
||||
ROS_ERROR("read_until error");
|
||||
}
|
||||
//=========================================================
|
||||
|
||||
// 检查信息头
|
||||
|
||||
if (buf[1] == usart_idf) {
|
||||
for(int i = 0; i < 4; i++)
|
||||
{ passball_.data[0] = buf[2];
|
||||
r2_x.data[i] = buf[i + 3];
|
||||
r2_y.data[i] = buf[i + 7];
|
||||
|
||||
}
|
||||
x = (double)r2_x.d;
|
||||
y = (double)r2_y.d;
|
||||
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void write1float(bool passball,double x, double y)
|
||||
{
|
||||
unsigned char buf[12] = {0};
|
||||
buf[0] = header_read; //buf[0]
|
||||
buf[1] = usart_idf; //buf[1]
|
||||
buf[11] = ender_read; //buf[10]
|
||||
buf[2] = passball;
|
||||
for(int i = 0; i < 4; i++)
|
||||
{
|
||||
buf[i + 3] = r1_x.data[i];
|
||||
buf[i + 7] = r1_y.data[i];
|
||||
}
|
||||
boost::asio::write(sp2, boost::asio::buffer(buf));
|
||||
}
|
||||
/********************************************************
|
||||
函数功能：获得8位循环冗余校验值
|
||||
入口参数：数组地址、长度
|
||||
出口参数：校验值
|
||||
********************************************************/
|
||||
unsigned char getCrc8(unsigned char *ptr, unsigned short len)
|
||||
{
|
||||
unsigned char crc;
|
||||
unsigned char i;
|
||||
crc = 0;
|
||||
while(len--)
|
||||
{
|
||||
crc ^= *ptr++;
|
||||
for(i = 0; i < 8; i++)
|
||||
{
|
||||
if(crc&0x01)
|
||||
crc=(crc>>1)^0x8C;
|
||||
else
|
||||
crc >>= 1;
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
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;
|
||||
}
|
||||
}
|
||||
15
src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h
Normal file → Executable file
15
src/FAST_LIO_LOCALIZATION/src/mbot_linux_serial.h
Normal file → Executable file
@ -8,11 +8,14 @@
|
||||
#include <nav_msgs/Odometry.h>
|
||||
#include <boost/asio.hpp>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
extern void write3float2(float x,float y,float yaw);
|
||||
extern void serialInit();
|
||||
extern void writeSpeed(double Left_v, double Right_v,unsigned char ctrlFlag);
|
||||
extern void 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
|
||||
|
||||
130
src/FAST_LIO_LOCALIZATION/src/position.cpp
Executable file
130
src/FAST_LIO_LOCALIZATION/src/position.cpp
Executable file
@ -0,0 +1,130 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <serial/serial.h>
|
||||
#include <ros/time.h>
|
||||
#include <fast_lio_localization/position.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/buffer.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <fast_lio_localization/underpan.h>
|
||||
static serial::Serial ser;
|
||||
static std::mutex serial_mutex;
|
||||
|
||||
class obtain_tf {
|
||||
public:
|
||||
obtain_tf(std::string fa, std::string ch) : tf_listener_(tf_buffer_), father_frame_(fa), child_frame_(ch) {
|
||||
fetchTransform(); // 在构造函数中调用 fetchTransform 方法
|
||||
}
|
||||
double x, y, yaw;
|
||||
double ob_x() {
|
||||
return x;
|
||||
}
|
||||
double ob_y() {
|
||||
return y;
|
||||
}
|
||||
double ob_yaw() {
|
||||
return yaw;
|
||||
}
|
||||
bool fetchTransform() {
|
||||
if (!tf_buffer_.canTransform(father_frame_, child_frame_, ros::Time(0), ros::Duration(1.0))) {
|
||||
ROS_WARN("等待变换: %s -> %s 不可用", father_frame_.c_str(), child_frame_.c_str());
|
||||
return false;
|
||||
}
|
||||
try {
|
||||
transform_ = tf_buffer_.lookupTransform(
|
||||
father_frame_,
|
||||
child_frame_,
|
||||
ros::Time(0)
|
||||
);
|
||||
x = transform_.transform.translation.x;
|
||||
y = transform_.transform.translation.y;
|
||||
yaw = suanyaw();
|
||||
return true;
|
||||
} catch (tf2::TransformException &ex) {
|
||||
ROS_ERROR("变换获取失败: %s", ex.what());
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
tf2_ros::Buffer tf_buffer_;
|
||||
tf2_ros::TransformListener tf_listener_;
|
||||
std::string father_frame_;
|
||||
std::string child_frame_;
|
||||
geometry_msgs::TransformStamped transform_;
|
||||
|
||||
|
||||
double suanyaw() {
|
||||
tf2::Quaternion q(
|
||||
transform_.transform.rotation.x,
|
||||
transform_.transform.rotation.y,
|
||||
transform_.transform.rotation.z,
|
||||
transform_.transform.rotation.w
|
||||
);
|
||||
double roll, pitch, yaw;
|
||||
tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
|
||||
return yaw;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
|
||||
// 串口参数
|
||||
void serialInit() {
|
||||
std::lock_guard<std::mutex> lock(serial_mutex);
|
||||
if (ser.isOpen()) ser.close();
|
||||
serial::Timeout to = serial::Timeout::simpleTimeout(100);
|
||||
ser.setPort("/dev/tor1");
|
||||
ser.setBaudrate(115200);
|
||||
ser.setTimeout(to);
|
||||
ser.open();
|
||||
if (ser.isOpen()) {
|
||||
ser.flushInput();
|
||||
ser.flushOutput();
|
||||
ROS_INFO("Serial port initialized and buffers cleared.");
|
||||
} else {
|
||||
ROS_ERROR("Failed to open serial port!tor1_position");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool write1float(double x,double y){
|
||||
std::lock_guard<std::mutex> lock(serial_mutex);
|
||||
if (!ser.isOpen()) return false;
|
||||
try {
|
||||
// 一帧总长度=帧头1+标志1+数据8+帧尾1=11字节
|
||||
std::string buf(11, 0);
|
||||
buf[0] = 0xFC; // 帧头
|
||||
buf[1] = 0x01; // 标志
|
||||
float x_f = static_cast<float>(x);
|
||||
float y_f = static_cast<float>(y);
|
||||
memcpy(&buf[2], &x_f, 4);
|
||||
memcpy(&buf[6], &y_f, 4);
|
||||
buf[10] = 0xFD; // 帧尾
|
||||
ser.write(buf);
|
||||
return true;
|
||||
} catch (...) {
|
||||
ROS_ERROR("Failed to write data to serial port.");
|
||||
}
|
||||
}
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "ros_serial_node");
|
||||
ros::NodeHandle nh;
|
||||
serialInit();
|
||||
|
||||
ros::Rate rate(1000);
|
||||
double x, y;
|
||||
bool o;
|
||||
obtain_tf tf_ob("map", "car");
|
||||
while (ros::ok()) {
|
||||
tf_ob.fetchTransform(); // 获取变换
|
||||
x = tf_ob.ob_x();
|
||||
y = tf_ob.ob_y();
|
||||
o = write1float(x,y);
|
||||
ROS_INFO("x: %f, y: %f,o=%d", x, y,int(o));
|
||||
rate.sleep();
|
||||
}
|
||||
}
|
||||
753
src/FAST_LIO_LOCALIZATION/src/preprocess.cpp
Normal file → Executable file
753
src/FAST_LIO_LOCALIZATION/src/preprocess.cpp
Normal file → Executable file
@ -3,11 +3,16 @@
|
||||
#define RETURN0 0x00
|
||||
#define RETURN0AND1 0x10
|
||||
|
||||
const bool time_list_cut_frame(PointType &x, PointType &y) {
|
||||
return (x.curvature < y.curvature);
|
||||
}
|
||||
|
||||
Preprocess::Preprocess()
|
||||
:feature_enabled(0), lidar_type(AVIA), blind(0.01), point_filter_num(1)
|
||||
:lidar_type(AVIA), blind(0.01), point_filter_num(1), det_range(1000)
|
||||
{
|
||||
inf_bound = 10;
|
||||
N_SCANS = 6;
|
||||
SCAN_RATE = 10;
|
||||
group_size = 8;
|
||||
disA = 0.01;
|
||||
disA = 0.1; // B?
|
||||
@ -34,7 +39,6 @@ Preprocess::~Preprocess() {}
|
||||
|
||||
void Preprocess::set(bool feat_en, int lid_type, double bld, int pfilt_num)
|
||||
{
|
||||
feature_enabled = feat_en;
|
||||
lidar_type = lid_type;
|
||||
blind = bld;
|
||||
point_filter_num = pfilt_num;
|
||||
@ -48,6 +52,25 @@ void Preprocess::process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, Poin
|
||||
|
||||
void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out)
|
||||
{
|
||||
switch (time_unit)
|
||||
{
|
||||
case SEC:
|
||||
time_unit_scale = 1.e3f;
|
||||
break;
|
||||
case MS:
|
||||
time_unit_scale = 1.f;
|
||||
break;
|
||||
case US:
|
||||
time_unit_scale = 1.e-3f;
|
||||
break;
|
||||
case NS:
|
||||
time_unit_scale = 1.e-6f;
|
||||
break;
|
||||
default:
|
||||
time_unit_scale = 1.f;
|
||||
break;
|
||||
}
|
||||
|
||||
switch (lidar_type)
|
||||
{
|
||||
case OUST64:
|
||||
@ -58,6 +81,10 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
|
||||
velodyne_handler(msg);
|
||||
break;
|
||||
|
||||
case HESAIxt32:
|
||||
hesai_handler(msg);
|
||||
break;
|
||||
|
||||
default:
|
||||
printf("Error LiDAR Type");
|
||||
break;
|
||||
@ -65,6 +92,227 @@ void Preprocess::process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointClo
|
||||
*pcl_out = pl_surf;
|
||||
}
|
||||
|
||||
void Preprocess::process_cut_frame_livox(const livox_ros_driver2::CustomMsg::ConstPtr &msg,
|
||||
deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar,
|
||||
const int required_frame_num, int scan_count) {
|
||||
int plsize = msg->point_num;
|
||||
pl_surf.clear();
|
||||
pl_surf.reserve(plsize);
|
||||
pl_full.clear();
|
||||
pl_full.resize(plsize);
|
||||
int valid_point_num = 0;
|
||||
|
||||
for (uint i = 1; i < plsize; i++) {
|
||||
if ((msg->points[i].line < N_SCANS) &&
|
||||
((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
|
||||
{
|
||||
valid_point_num++;
|
||||
if (valid_point_num % point_filter_num == 0) {
|
||||
pl_full[i].x = msg->points[i].x;
|
||||
pl_full[i].y = msg->points[i].y;
|
||||
pl_full[i].z = msg->points[i].z;
|
||||
pl_full[i].intensity = msg->points[i].reflectivity;
|
||||
//use curvature as time of each laser points，unit: ms
|
||||
pl_full[i].curvature = msg->points[i].offset_time / float(1000000);
|
||||
|
||||
double dist = pl_full[i].x * pl_full[i].x + pl_full[i].y * pl_full[i].y + pl_full[i].z * pl_full[i].z;
|
||||
if (dist < blind * blind || dist > det_range * det_range) continue;
|
||||
|
||||
if ((abs(pl_full[i].x - pl_full[i - 1].x) > 1e-7)
|
||||
|| (abs(pl_full[i].y - pl_full[i - 1].y) > 1e-7)
|
||||
|| (abs(pl_full[i].z - pl_full[i - 1].z) > 1e-7)) {
|
||||
pl_surf.push_back(pl_full[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);
|
||||
//end time of last frame，单位ms
|
||||
double last_frame_end_time = msg->header.stamp.toSec() * 1000;
|
||||
uint valid_num = 0;
|
||||
uint cut_num = 0;
|
||||
uint valid_pcl_size = pl_surf.points.size();
|
||||
|
||||
int required_cut_num = required_frame_num;
|
||||
if (scan_count < 5)
|
||||
required_cut_num = 1;
|
||||
|
||||
PointCloudXYZI pcl_cut;
|
||||
for (uint i = 1; i < valid_pcl_size; i++) {
|
||||
valid_num++;
|
||||
//Compute new opffset time of each point：ms
|
||||
pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time;
|
||||
pcl_cut.push_back(pl_surf[i]);
|
||||
if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {
|
||||
cut_num++;
|
||||
time_lidar.push_back(last_frame_end_time);
|
||||
PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI()); //Initialize shared_ptr
|
||||
*pcl_temp = pcl_cut;
|
||||
pcl_out.push_back(pcl_temp);
|
||||
//Update frame head
|
||||
last_frame_end_time += pl_surf[i].curvature;
|
||||
pcl_cut.clear();
|
||||
pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num);
|
||||
}
|
||||
}
|
||||
}
|
||||
#define MAX_LINE_NUM 128
|
||||
void
|
||||
Preprocess::process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out,
|
||||
deque<double> &time_lidar, const int required_frame_num, int scan_count) {
|
||||
pl_surf.clear();
|
||||
pl_corn.clear();
|
||||
pl_full.clear();
|
||||
if (lidar_type == VELO16) {
|
||||
pcl::PointCloud<velodyne_ros::Point> pl_orig;
|
||||
pcl::fromROSMsg(*msg, pl_orig);
|
||||
int plsize = pl_orig.points.size();
|
||||
pl_surf.reserve(plsize);
|
||||
|
||||
bool is_first[MAX_LINE_NUM];
|
||||
double yaw_fp[MAX_LINE_NUM] = {0}; // yaw of first scan point
|
||||
double omega_l = 3.61; // scan angular velocity (deg/ms)
|
||||
float yaw_last[MAX_LINE_NUM] = {0.0}; // yaw of last scan point
|
||||
float time_last[MAX_LINE_NUM] = {0.0}; // last offset time
|
||||
|
||||
if (pl_orig.points[plsize - 1].time > 0) {
|
||||
given_offset_time = true;
|
||||
} else {
|
||||
cout << "Compute offset time using constant rotation model." << endl;
|
||||
given_offset_time = false;
|
||||
memset(is_first, true, sizeof(is_first));
|
||||
}
|
||||
|
||||
for (int i = 0; i < plsize; i++) {
|
||||
PointType added_pt;
|
||||
added_pt.normal_x = 0;
|
||||
added_pt.normal_y = 0;
|
||||
added_pt.normal_z = 0;
|
||||
added_pt.x = pl_orig.points[i].x;
|
||||
added_pt.y = pl_orig.points[i].y;
|
||||
added_pt.z = pl_orig.points[i].z;
|
||||
added_pt.intensity = pl_orig.points[i].intensity;
|
||||
added_pt.curvature = pl_orig.points[i].time * 1000.0; //ms
|
||||
|
||||
double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
|
||||
if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))
|
||||
continue;
|
||||
|
||||
if (!given_offset_time) {
|
||||
int layer = pl_orig.points[i].ring;
|
||||
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
|
||||
|
||||
if (is_first[layer]) {
|
||||
yaw_fp[layer] = yaw_angle;
|
||||
is_first[layer] = false;
|
||||
added_pt.curvature = 0.0;
|
||||
yaw_last[layer] = yaw_angle;
|
||||
time_last[layer] = added_pt.curvature;
|
||||
continue;
|
||||
}
|
||||
// compute offset time
|
||||
if (yaw_angle <= yaw_fp[layer]) {
|
||||
added_pt.curvature = (yaw_fp[layer] - yaw_angle) / omega_l;
|
||||
} else {
|
||||
added_pt.curvature = (yaw_fp[layer] - yaw_angle + 360.0) / omega_l;
|
||||
}
|
||||
if (added_pt.curvature < time_last[layer]) added_pt.curvature += 360.0 / omega_l;
|
||||
|
||||
yaw_last[layer] = yaw_angle;
|
||||
time_last[layer] = added_pt.curvature;
|
||||
}
|
||||
|
||||
|
||||
if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {
|
||||
pl_surf.points.push_back(added_pt);
|
||||
}
|
||||
}
|
||||
} else if (lidar_type == OUST64) {
|
||||
pcl::PointCloud<ouster_ros::Point> pl_orig;
|
||||
pcl::fromROSMsg(*msg, pl_orig);
|
||||
int plsize = pl_orig.points.size();
|
||||
pl_surf.reserve(plsize);
|
||||
for (int i = 0; i < plsize; i++) {
|
||||
PointType added_pt;
|
||||
added_pt.normal_x = 0;
|
||||
added_pt.normal_y = 0;
|
||||
added_pt.normal_z = 0;
|
||||
added_pt.x = pl_orig.points[i].x;
|
||||
added_pt.y = pl_orig.points[i].y;
|
||||
added_pt.z = pl_orig.points[i].z;
|
||||
added_pt.intensity = pl_orig.points[i].intensity;
|
||||
added_pt.curvature = pl_orig.points[i].t / 1e6; //ns to ms
|
||||
|
||||
double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
|
||||
if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))
|
||||
continue;
|
||||
|
||||
if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {
|
||||
pl_surf.points.push_back(added_pt);
|
||||
}
|
||||
}
|
||||
} else if(lidar_type == HESAIxt32) {
|
||||
pcl::PointCloud<hesai_ros::Point> pl_orig;
|
||||
pcl::fromROSMsg(*msg, pl_orig);
|
||||
int plsize = pl_orig.points.size();
|
||||
pl_surf.reserve(plsize);
|
||||
for (int i = 0; i < plsize; i++) {
|
||||
PointType added_pt;
|
||||
added_pt.normal_x = 0;
|
||||
added_pt.normal_y = 0;
|
||||
added_pt.normal_z = 0;
|
||||
added_pt.x = pl_orig.points[i].x;
|
||||
added_pt.y = pl_orig.points[i].y;
|
||||
added_pt.z = pl_orig.points[i].z;
|
||||
added_pt.intensity = pl_orig.points[i].intensity;
|
||||
added_pt.curvature = (pl_orig.points[i].timestamp - msg->header.stamp.toSec()) * 1000; //s to ms
|
||||
|
||||
double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
|
||||
if ( dist < blind * blind || dist > det_range * det_range || isnan(added_pt.x) || isnan(added_pt.y) || isnan(added_pt.z))
|
||||
continue;
|
||||
|
||||
if (i % point_filter_num == 0 && pl_orig.points[i].ring < N_SCANS) {
|
||||
pl_surf.points.push_back(added_pt);
|
||||
}
|
||||
}
|
||||
}else{
|
||||
cout << "Wrong LiDAR Type!!!" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
sort(pl_surf.points.begin(), pl_surf.points.end(), time_list_cut_frame);
|
||||
|
||||
//ms
|
||||
double last_frame_end_time = msg->header.stamp.toSec() * 1000;
|
||||
uint valid_num = 0;
|
||||
uint cut_num = 0;
|
||||
uint valid_pcl_size = pl_surf.points.size();
|
||||
|
||||
int required_cut_num = required_frame_num;
|
||||
|
||||
if (scan_count < 20)
|
||||
required_cut_num = 1;
|
||||
|
||||
|
||||
PointCloudXYZI pcl_cut;
|
||||
for (uint i = 1; i < valid_pcl_size; i++) {
|
||||
valid_num++;
|
||||
pl_surf[i].curvature += msg->header.stamp.toSec() * 1000 - last_frame_end_time;
|
||||
pcl_cut.push_back(pl_surf[i]);
|
||||
|
||||
if (valid_num == (int((cut_num + 1) * valid_pcl_size / required_cut_num) - 1)) {
|
||||
cut_num++;
|
||||
time_lidar.push_back(last_frame_end_time);
|
||||
PointCloudXYZI::Ptr pcl_temp(new PointCloudXYZI());
|
||||
*pcl_temp = pcl_cut;
|
||||
pcl_out.push_back(pcl_temp);
|
||||
last_frame_end_time += pl_surf[i].curvature;
|
||||
pcl_cut.clear();
|
||||
pcl_cut.reserve(valid_pcl_size * 2 / required_frame_num);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
|
||||
{
|
||||
pl_surf.clear();
|
||||
@ -72,7 +320,6 @@ void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
|
||||
pl_full.clear();
|
||||
double t1 = omp_get_wtime();
|
||||
int plsize = msg->point_num;
|
||||
// cout<<"plsie: "<<plsize<<endl;
|
||||
|
||||
pl_corn.reserve(plsize);
|
||||
pl_surf.reserve(plsize);
|
||||
@ -85,84 +332,31 @@ void Preprocess::avia_handler(const livox_ros_driver2::CustomMsg::ConstPtr &msg)
|
||||
}
|
||||
uint valid_num = 0;
|
||||
|
||||
if (feature_enabled)
|
||||
|
||||
for(uint i=1; i<plsize; i++)
|
||||
{
|
||||
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))
|
||||
{
|
||||
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<N_SCANS; j++)
|
||||
{
|
||||
if(pl_buff[j].size() <= 5) continue;
|
||||
pcl::PointCloud<PointType> &pl = pl_buff[j];
|
||||
plsize = pl.size();
|
||||
vector<orgtype> &types = typess[j];
|
||||
types.clear();
|
||||
types.resize(plsize);
|
||||
plsize--;
|
||||
for(uint i=0; i<plsize; i++)
|
||||
{
|
||||
types[i].range = pl[i].x * pl[i].x + pl[i].y * pl[i].y;
|
||||
vx = pl[i].x - pl[i + 1].x;
|
||||
vy = pl[i].y - pl[i + 1].y;
|
||||
vz = pl[i].z - pl[i + 1].z;
|
||||
types[i].dista = vx * vx + vy * vy + vz * vz;
|
||||
}
|
||||
types[plsize].range = pl[plsize].x * pl[plsize].x + pl[plsize].y * pl[plsize].y;
|
||||
give_feature(pl, types);
|
||||
// pl_surf += pl;
|
||||
}
|
||||
time += omp_get_wtime() - t0;
|
||||
printf("Feature extraction time: %lf \n", time / count);
|
||||
}
|
||||
else
|
||||
{
|
||||
for(uint i=1; i<plsize; i++)
|
||||
{
|
||||
if((msg->points[i].line < N_SCANS) && ((msg->points[i].tag & 0x30) == 0x10 || (msg->points[i].tag & 0x30) == 0x00))
|
||||
{
|
||||
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<orgtype> &types = typess[j];
|
||||
types.clear();
|
||||
types.resize(linesize);
|
||||
linesize--;
|
||||
for (uint i = 0; i < linesize; i++)
|
||||
{
|
||||
types[i].range = sqrt(pl[i].x * pl[i].x + pl[i].y * pl[i].y);
|
||||
vx = pl[i].x - pl[i + 1].x;
|
||||
vy = pl[i].y - pl[i + 1].y;
|
||||
vz = pl[i].z - pl[i + 1].z;
|
||||
types[i].dista = vx * vx + vy * vy + vz * vz;
|
||||
}
|
||||
types[linesize].range = sqrt(pl[linesize].x * pl[linesize].x + pl[linesize].y * pl[linesize].y);
|
||||
give_feature(pl, types);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
double time_stamp = msg->header.stamp.toSec();
|
||||
// cout << "===================================" << endl;
|
||||
// printf("Pt size = %d, N_SCANS = %d\r\n", plsize, N_SCANS);
|
||||
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<velodyne_ros::Point> pl_orig;
|
||||
pcl::fromROSMsg(*msg, pl_orig);
|
||||
int plsize = pl_orig.points.size();
|
||||
if (plsize == 0) return;
|
||||
pl_surf.reserve(plsize);
|
||||
|
||||
|
||||
/*** These variables only works when no point timestamps given ***/
|
||||
double omega_l=3.61; // scan angular velocity
|
||||
double omega_l = 0.361 * SCAN_RATE; // scan angular velocity
|
||||
std::vector<bool> is_first(N_SCANS,true);
|
||||
std::vector<double> yaw_fp(N_SCANS, 0.0); // yaw of first scan point
|
||||
std::vector<float> yaw_last(N_SCANS, 0.0); // yaw of last scan point
|
||||
@ -290,6 +425,111 @@ void Preprocess::velodyne_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
|
||||
given_offset_time = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
given_offset_time = false;
|
||||
// double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
|
||||
// double yaw_end = yaw_first;
|
||||
// int layer_first = pl_orig.points[0].ring;
|
||||
// for (uint i = plsize - 1; i > 0; i--)
|
||||
// {
|
||||
// if (pl_orig.points[i].ring == layer_first)
|
||||
// {
|
||||
// yaw_end = atan2(pl_orig.points[i].y, pl_orig.points[i].x) * 57.29578;
|
||||
// break;
|
||||
// }
|
||||
// }
|
||||
}
|
||||
|
||||
for (int i = 0; i < plsize; i++)
|
||||
{
|
||||
PointType added_pt;
|
||||
// cout<<"!!!!!!"<<i<<" "<<plsize<<endl;
|
||||
|
||||
added_pt.normal_x = 0;
|
||||
added_pt.normal_y = 0;
|
||||
added_pt.normal_z = 0;
|
||||
added_pt.x = pl_orig.points[i].x;
|
||||
added_pt.y = pl_orig.points[i].y;
|
||||
added_pt.z = pl_orig.points[i].z;
|
||||
added_pt.intensity = pl_orig.points[i].intensity;
|
||||
added_pt.curvature = pl_orig.points[i].time * time_unit_scale; // curvature unit: ms // cout<<added_pt.curvature<<endl;
|
||||
if (i % point_filter_num != 0 || std::isnan(added_pt.x) || std::isnan(added_pt.y) || std::isnan(added_pt.z)) continue;
|
||||
|
||||
if (!given_offset_time)
|
||||
{
|
||||
// int unit_size = plsize / 16;
|
||||
// int layer = i / unit_size; // pl_orig.points[i].ring;
|
||||
// cout << "check layer:" << unit_size << ";" << i << ";" << layer << endl;
|
||||
int layer = 0;
|
||||
double yaw_angle = atan2(added_pt.y, added_pt.x) * 57.2957;
|
||||
|
||||
if (is_first[layer])
|
||||
{
|
||||
// printf("layer: %d; is first: %d", layer, is_first[layer]);
|
||||
yaw_fp[layer]=yaw_angle;
|
||||
is_first[layer]=false;
|
||||
added_pt.curvature = 0.0;
|
||||
yaw_last[layer]=yaw_angle;
|
||||
time_last[layer]=added_pt.curvature;
|
||||
continue;
|
||||
}
|
||||
|
||||
// compute offset time
|
||||
if (yaw_angle < yaw_fp[layer])
|
||||
{
|
||||
added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
|
||||
// added_pt.curvature = (yaw_angle + 360.0 - yaw_fp[layer]) / omega_l;
|
||||
}
|
||||
else
|
||||
{
|
||||
added_pt.curvature = (yaw_fp[layer]-yaw_angle+360.0) / omega_l;
|
||||
// added_pt.curvature = (yaw_angle-yaw_fp[layer]) / omega_l;
|
||||
}
|
||||
|
||||
// if (added_pt.curvature < time_last[layer]) added_pt.curvature+=360.0/omega_l;
|
||||
|
||||
// yaw_last[layer] = yaw_angle;
|
||||
// time_last[layer]=added_pt.curvature;
|
||||
}
|
||||
|
||||
// if (i % point_filter_num == 0 && !std::isnan(added_pt.x) && !std::isnan(added_pt.y) && !std::isnan(added_pt.z))
|
||||
double dist = added_pt.x * added_pt.x + added_pt.y * added_pt.y + added_pt.z * added_pt.z;
|
||||
{
|
||||
if(dist > (blind * blind)
|
||||
&& dist < (det_range * det_range))
|
||||
{
|
||||
pl_surf.points.push_back(added_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Preprocess::hesai_handler(const sensor_msgs::PointCloud2::ConstPtr &msg)
|
||||
{
|
||||
pl_surf.clear();
|
||||
pl_corn.clear();
|
||||
pl_full.clear();
|
||||
|
||||
pcl::PointCloud<hesai_ros::Point> pl_orig;
|
||||
pcl::fromROSMsg(*msg, pl_orig);
|
||||
int plsize = pl_orig.points.size();
|
||||
if (plsize == 0) return;
|
||||
pl_surf.reserve(plsize);
|
||||
|
||||
/*** These variables only works when no point timestamps given ***/
|
||||
double omega_l = 0.361 * SCAN_RATE; // scan angular velocity
|
||||
std::vector<bool> is_first(N_SCANS,true);
|
||||
std::vector<double> yaw_fp(N_SCANS, 0.0); // yaw of first scan point
|
||||
std::vector<float> yaw_last(N_SCANS, 0.0); // yaw of last scan point
|
||||
std::vector<float> time_last(N_SCANS, 0.0); // last offset time
|
||||
/*****************************************************************/
|
||||
|
||||
if (pl_orig.points[plsize - 1].timestamp > 0)
|
||||
{
|
||||
given_offset_time = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
given_offset_time = false;
|
||||
double yaw_first = atan2(pl_orig.points[0].y, pl_orig.points[0].x) * 57.29578;
|
||||
@ -305,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<<"!!!!!!"<<i<<" "<<plsize<<endl;
|
||||
|
||||
for (int i = 0; i < plsize; i++)
|
||||
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 - time_head) * time_unit_scale; // curvature unit: ms // cout<<added_pt.curvature<<endl;
|
||||
if (!given_offset_time)
|
||||
{
|
||||
PointType added_pt;
|
||||
added_pt.normal_x = 0;
|
||||
added_pt.normal_y = 0;
|
||||
added_pt.normal_z = 0;
|
||||
int layer = pl_orig.points[i].ring;
|
||||
if (layer >= N_SCANS) continue;
|
||||
added_pt.x = pl_orig.points[i].x;
|
||||
added_pt.y = pl_orig.points[i].y;
|
||||
added_pt.z = pl_orig.points[i].z;
|
||||
added_pt.intensity = pl_orig.points[i].intensity;
|
||||
added_pt.curvature = pl_orig.points[i].time / 1000.0; // units: ms
|
||||
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<orgtype> &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<<"!!!!!!"<<i<<" "<<plsize<<endl;
|
||||
|
||||
added_pt.normal_x = 0;
|
||||
added_pt.normal_y = 0;
|
||||
added_pt.normal_z = 0;
|
||||
added_pt.x = pl_orig.points[i].x;
|
||||
added_pt.y = pl_orig.points[i].y;
|
||||
added_pt.z = pl_orig.points[i].z;
|
||||
added_pt.intensity = pl_orig.points[i].intensity;
|
||||
added_pt.curvature = pl_orig.points[i].time / 1000.0;
|
||||
|
||||
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])
|
||||
{
|
||||
// printf("layer: %d; is first: %d", layer, is_first[layer]);
|
||||
yaw_fp[layer]=yaw_angle;
|
||||
is_first[layer]=false;
|
||||
added_pt.curvature = 0.0;
|
||||
yaw_last[layer]=yaw_angle;
|
||||
time_last[layer]=added_pt.curvature;
|
||||
continue;
|
||||
}
|
||||
|
||||
// compute offset time
|
||||
if (yaw_angle <= yaw_fp[layer])
|
||||
{
|
||||
added_pt.curvature = (yaw_fp[layer]-yaw_angle) / omega_l;
|
||||
}
|
||||
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)
|
||||
{
|
||||
if(added_pt.x*added_pt.x+added_pt.y*added_pt.y+added_pt.z*added_pt.z > blind)
|
||||
{
|
||||
pl_surf.points.push_back(added_pt);
|
||||
}
|
||||
pl_surf.points.push_back(added_pt);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &types)
|
||||
@ -513,52 +678,6 @@ void Preprocess::give_feature(pcl::PointCloud<PointType> &pl, vector<orgtype> &t
|
||||
i = i_nex;
|
||||
last_state = 0;
|
||||
}
|
||||
// else if(plane_type == 0)
|
||||
// {
|
||||
// if(last_state == 1)
|
||||
// {
|
||||
// uint i_nex_tem;
|
||||
// uint j;
|
||||
// for(j=last_i+1; j<=last_i_nex; j++)
|
||||
// {
|
||||
// uint i_nex_tem2 = i_nex_tem;
|
||||
// Eigen::Vector3d curr_direct2;
|
||||
|
||||
// uint ttem = plane_judge(pl, types, j, i_nex_tem, curr_direct2);
|
||||
|
||||
// if(ttem != 1)
|
||||
// {
|
||||
// i_nex_tem = i_nex_tem2;
|
||||
// break;
|
||||
// }
|
||||
// curr_direct = curr_direct2;
|
||||
// }
|
||||
|
||||
// if(j == last_i+1)
|
||||
// {
|
||||
// last_state = 0;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// for(uint k=last_i_nex; k<=i_nex_tem; k++)
|
||||
// {
|
||||
// if(k != i_nex_tem)
|
||||
// {
|
||||
// types[k].ftype = Real_Plane;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// types[k].ftype = Poss_Plane;
|
||||
// }
|
||||
// }
|
||||
// i = i_nex_tem-1;
|
||||
// i_nex = i_nex_tem;
|
||||
// i2 = j-1;
|
||||
// last_state = 1;
|
||||
// }
|
||||
|
||||
// }
|
||||
// }
|
||||
|
||||
last_i = i2;
|
||||
last_i_nex = i_nex;
|
||||
|
||||
38
src/FAST_LIO_LOCALIZATION/src/preprocess.h
Normal file → Executable file
38
src/FAST_LIO_LOCALIZATION/src/preprocess.h
Normal file → Executable file
@ -10,11 +10,14 @@ using namespace std;
|
||||
typedef pcl::PointXYZINormal PointType;
|
||||
typedef pcl::PointCloud<PointType> PointCloudXYZI;
|
||||
|
||||
enum LID_TYPE{AVIA = 1, VELO16, OUST64}; //{1, 2, 3}
|
||||
enum LID_TYPE{AVIA = 1, VELO16, OUST64, HESAIxt32}; //{1, 2, 3, 4}
|
||||
enum TIME_UNIT{SEC = 0, MS = 1, US = 2, NS = 3};
|
||||
enum Feature{Nor, Poss_Plane, Real_Plane, Edge_Jump, Edge_Plane, Wire, ZeroPoint};
|
||||
enum Surround{Prev, Next};
|
||||
enum E_jump{Nr_nor, Nr_zero, Nr_180, Nr_inf, Nr_blind};
|
||||
|
||||
const bool time_list_cut_frame(PointType &x, PointType &y);
|
||||
|
||||
struct orgtype
|
||||
{
|
||||
double range;
|
||||
@ -48,7 +51,25 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(velodyne_ros::Point,
|
||||
(float, z, z)
|
||||
(float, intensity, intensity)
|
||||
(float, time, time)
|
||||
(uint16_t, ring, ring)
|
||||
(std::uint16_t, ring, ring)
|
||||
)
|
||||
|
||||
namespace hesai_ros {
|
||||
struct EIGEN_ALIGN16 Point {
|
||||
PCL_ADD_POINT4D;
|
||||
float intensity;
|
||||
double timestamp;
|
||||
uint16_t ring;
|
||||
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
||||
};
|
||||
} // namespace velodyne_ros
|
||||
POINT_CLOUD_REGISTER_POINT_STRUCT(hesai_ros::Point,
|
||||
(float, x, x)
|
||||
(float, y, y)
|
||||
(float, z, z)
|
||||
(float, intensity, intensity)
|
||||
(double, timestamp, timestamp)
|
||||
(std::uint16_t, ring, ring)
|
||||
)
|
||||
|
||||
namespace ouster_ros {
|
||||
@ -86,6 +107,10 @@ class Preprocess
|
||||
Preprocess();
|
||||
~Preprocess();
|
||||
|
||||
void process_cut_frame_livox(const livox_ros_driver2::CustomMsg::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
|
||||
|
||||
void process_cut_frame_pcl2(const sensor_msgs::PointCloud2::ConstPtr &msg, deque<PointCloudXYZI::Ptr> &pcl_out, deque<double> &time_lidar, const int required_frame_num, int scan_count);
|
||||
|
||||
void process(const livox_ros_driver2::CustomMsg::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
|
||||
void process(const sensor_msgs::PointCloud2::ConstPtr &msg, PointCloudXYZI::Ptr &pcl_out);
|
||||
void set(bool feat_en, int lid_type, double bld, int pfilt_num);
|
||||
@ -94,10 +119,10 @@ class Preprocess
|
||||
PointCloudXYZI pl_full, pl_corn, pl_surf;
|
||||
PointCloudXYZI pl_buff[128]; //maximum 128 line lidar
|
||||
vector<orgtype> typess[128]; //maximum 128 line lidar
|
||||
int lidar_type, point_filter_num, N_SCANS;;
|
||||
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<orgtype> &types);
|
||||
void pub_func(PointCloudXYZI &pl, const ros::Time &ct);
|
||||
int plane_judge(const PointCloudXYZI &pl, vector<orgtype> &types, uint i, uint &i_nex, Eigen::Vector3d &curr_direct);
|
||||
|
||||
195
src/FAST_LIO_LOCALIZATION/src/send.cpp
Executable file
195
src/FAST_LIO_LOCALIZATION/src/send.cpp
Executable file
@ -0,0 +1,195 @@
|
||||
#include <ros/ros.h>
|
||||
#include <fast_lio_localization/GoalPose.h>
|
||||
#include <tf/transform_broadcaster.h>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "send_pose");
|
||||
ros::NodeHandle nh;
|
||||
|
||||
// 创建预定义目标点列表
|
||||
std::vector<fast_lio_localization::GoalPose> pose_list(100);
|
||||
|
||||
// 初始化默认坐标点
|
||||
// 点1：原点
|
||||
|
||||
// 点2：前进1米
|
||||
|
||||
pose_list[0].x =-3.9140;//-4.657,-4.660 4.662,4.644,4.657
|
||||
pose_list[0].y =-5.9587;//-5.948,-5.957,-5.968,-5.965,-5.957
|
||||
pose_list[0].angle =-1.741;
|
||||
pose_list[0].max_speed = 5.0;
|
||||
pose_list[0].tolerance = 0.00;
|
||||
pose_list[0].rotor = 0.0;
|
||||
pose_list[0].circle = -1;
|
||||
|
||||
pose_list[1].x =-3.9140;//-4.657,-4.660 4.662,4.644,4.657
|
||||
pose_list[1].y =-5.9587;//-5.948,-5.957,-5.968,-5.965,-5.957
|
||||
pose_list[1].angle =-1.741;
|
||||
pose_list[1].max_speed = 5.0;
|
||||
pose_list[1].tolerance = 0.00;
|
||||
pose_list[1].rotor = 0.0;
|
||||
pose_list[1].circle = 0;
|
||||
// 点3：侧向移动1米，旋转90度
|
||||
|
||||
pose_list[2].y =-1.0;
|
||||
pose_list[2].angle =0 ;
|
||||
pose_list[2].max_speed = 8.0;
|
||||
pose_list[2].tolerance = 0.00;
|
||||
pose_list[2].rotor = 0.0;
|
||||
pose_list[2].circle = 0.0;
|
||||
|
||||
|
||||
|
||||
pose_list[3].x=-3.914;
|
||||
pose_list[3].y=-5.9587;
|
||||
pose_list[3].angle =0 ;
|
||||
pose_list[3].max_speed = 4.0;
|
||||
pose_list[3].tolerance = 0.00;
|
||||
pose_list[3].rotor = 0.0;
|
||||
pose_list[3].circle = 3.5;
|
||||
|
||||
pose_list[4].x=-3.914;
|
||||
pose_list[4].y=-5.9587;
|
||||
pose_list[4].angle =0;
|
||||
pose_list[4].max_speed = 4.0;
|
||||
pose_list[4].tolerance = 0.00;
|
||||
pose_list[4].rotor = 0.0;
|
||||
pose_list[4].circle = 3.8;
|
||||
|
||||
pose_list[5].x=-3.914;
|
||||
pose_list[5].y=-5.9587;
|
||||
pose_list[5].angle =0;
|
||||
pose_list[5].max_speed = 4.0;
|
||||
pose_list[5].tolerance = 0.00;
|
||||
pose_list[5].rotor = 0.0;
|
||||
pose_list[5].circle = 4;
|
||||
|
||||
pose_list[6].x=-3.914;
|
||||
pose_list[6].y=-5.9587;
|
||||
pose_list[6].angle =0;
|
||||
pose_list[6].max_speed = 4.0;
|
||||
pose_list[6].tolerance = 0.0;
|
||||
pose_list[6].rotor = 0.0;
|
||||
pose_list[6].circle = 4.5;
|
||||
|
||||
pose_list[7].x=-3.914;
|
||||
pose_list[7].y=-5.9587;
|
||||
pose_list[7].angle =0;
|
||||
pose_list[7].max_speed = 4.0;
|
||||
pose_list[7].tolerance = 0.0;
|
||||
pose_list[7].rotor = 0.0;
|
||||
pose_list[7].circle = 5;
|
||||
|
||||
pose_list[8].x=-3.914;
|
||||
pose_list[8].y=-5.9587;
|
||||
pose_list[8].angle =0;
|
||||
pose_list[8].max_speed = 4.0;
|
||||
pose_list[8].tolerance = 0.0;
|
||||
pose_list[8].rotor = 0.0;
|
||||
pose_list[8].circle = 5.5;
|
||||
|
||||
pose_list[9].x=-3.914;
|
||||
pose_list[9].y=-5.9587;
|
||||
pose_list[9].angle =-1.57 ;
|
||||
pose_list[9].max_speed = 4.0;
|
||||
pose_list[9].tolerance = 0.0;
|
||||
pose_list[9].rotor = 0.0;
|
||||
pose_list[9].circle = 6.0;
|
||||
|
||||
pose_list[10].x=-3.914;
|
||||
pose_list[10].y=-5.9587;
|
||||
pose_list[10].angle =0;
|
||||
pose_list[10].max_speed = 4.0;
|
||||
pose_list[10].tolerance = 0.0;
|
||||
pose_list[10].rotor = 0.0;
|
||||
pose_list[10].circle = 4.2;
|
||||
|
||||
pose_list[11].x=-3.914;
|
||||
pose_list[11].y=-5.9587;
|
||||
pose_list[11].angle =0;
|
||||
pose_list[11].max_speed = 4.0;
|
||||
pose_list[11].tolerance = 0.0;
|
||||
pose_list[11].rotor = 0.0;
|
||||
pose_list[11].circle = 4.7;
|
||||
|
||||
pose_list[12].x=-3.914;
|
||||
pose_list[12].y=-5.9587;
|
||||
pose_list[12].angle =0;
|
||||
pose_list[12].max_speed = 4.0;
|
||||
pose_list[12].tolerance = 0.0;
|
||||
pose_list[12].rotor = 0.0;
|
||||
pose_list[12].circle = 5.2;
|
||||
|
||||
pose_list[13].x=-3.914;
|
||||
pose_list[13].y=-5.9587;
|
||||
pose_list[13].angle =0;
|
||||
pose_list[13].max_speed = 4.0;
|
||||
pose_list[13].tolerance = 0.0;
|
||||
pose_list[13].rotor = 0.0;
|
||||
pose_list[13].circle = 5.7;
|
||||
|
||||
pose_list[14].x=-3.914;
|
||||
pose_list[14].y=-5.9587;
|
||||
pose_list[14].angle =0;
|
||||
pose_list[14].max_speed = 4.0;
|
||||
pose_list[14].tolerance = 0.0;
|
||||
pose_list[14].rotor = 0.0;
|
||||
pose_list[14].circle = -1;
|
||||
|
||||
// 参数处理
|
||||
int selected_index = 0; // 默认选择第一个点
|
||||
if(argc >= 2)
|
||||
{
|
||||
try {
|
||||
selected_index = std::stoi(argv[1]);
|
||||
if(selected_index < 0 || selected_index >= pose_list.size())
|
||||
{
|
||||
ROS_WARN("Invalid index %d, using default pose 0", selected_index);
|
||||
selected_index = 0;
|
||||
}
|
||||
}
|
||||
catch(...) {
|
||||
ROS_ERROR("Failed to parse index argument");
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
|
||||
ROS_INFO("Selected pose index: %d", selected_index);
|
||||
|
||||
ros::Publisher pose_publisher = nh.advertise<fast_lio_localization::GoalPose>("/goal_pose", 10);
|
||||
tf::TransformBroadcaster tf_broadcaster;
|
||||
ros::Rate rate(10);
|
||||
|
||||
const auto& goal_pose = pose_list[selected_index]; // 获取选中的目标点
|
||||
|
||||
while(ros::ok())
|
||||
{
|
||||
// 发布目标位姿消息
|
||||
pose_publisher.publish(goal_pose);
|
||||
|
||||
// 创建并填充坐标系变换
|
||||
geometry_msgs::TransformStamped transform;
|
||||
transform.header.stamp = ros::Time::now();
|
||||
transform.header.frame_id = "map";
|
||||
transform.child_frame_id = "goal_pose";
|
||||
|
||||
transform.transform.translation.x = goal_pose.x;
|
||||
transform.transform.translation.y = goal_pose.y;
|
||||
transform.transform.translation.z = 0.0;
|
||||
|
||||
tf::Quaternion q = tf::createQuaternionFromYaw(goal_pose.angle);
|
||||
transform.transform.rotation.x = q.x();
|
||||
transform.transform.rotation.y = q.y();
|
||||
transform.transform.rotation.z = q.z();
|
||||
transform.transform.rotation.w = q.w();
|
||||
|
||||
// 广播变换
|
||||
tf_broadcaster.sendTransform(transform);
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
84
src/FAST_LIO_LOCALIZATION/src/test_goal.cpp
Executable file
84
src/FAST_LIO_LOCALIZATION/src/test_goal.cpp
Executable file
@ -0,0 +1,84 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2_ros/transform_listener.h>
|
||||
#include <tf2_ros/static_transform_broadcaster.h>
|
||||
#include <tf2/utils.h> // 关键：添加四元数转换工具
|
||||
#include <geometry_msgs/PoseStamped.h>
|
||||
#include <geometry_msgs/TransformStamped.h>
|
||||
#include <fast_lio_localization/GoalPose.h>
|
||||
#include <fast_lio_localization/DataAI.h>
|
||||
#include <fast_lio_localization/DataNav.h>
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
ros::init(argc, argv, "fixed_goal_pose_publisher");
|
||||
ros::NodeHandle nh("~");
|
||||
|
||||
// 参数配置
|
||||
std::string car_frame = "car";
|
||||
std::string map_frame = "bash";
|
||||
nh.param<std::string>("car", car_frame, "car");
|
||||
nh.param<std::string>("bash", map_frame, "bash");
|
||||
|
||||
// 初始化TF监听器
|
||||
tf2_ros::Buffer tf_buffer;
|
||||
tf2_ros::TransformListener tf_listener(tf_buffer);
|
||||
|
||||
// 等待并获取初始位姿
|
||||
ROS_INFO("等待 %s 到 %s 的变换...", map_frame.c_str(), car_frame.c_str());
|
||||
geometry_msgs::TransformStamped transform;
|
||||
try {
|
||||
transform = tf_buffer.lookupTransform(
|
||||
map_frame, // target frame
|
||||
car_frame, // source frame
|
||||
ros::Time(0), // 获取最新可用时间
|
||||
ros::Duration(10.0) // 超时时间
|
||||
);
|
||||
ROS_INFO("成功获取初始位姿！");
|
||||
} catch (tf2::TransformException &ex) {
|
||||
ROS_ERROR("变换获取失败: %s", ex.what());
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 创建静态坐标系发布器
|
||||
static tf2_ros::StaticTransformBroadcaster static_broadcaster;
|
||||
geometry_msgs::TransformStamped static_transform;
|
||||
static_transform.header.stamp = ros::Time::now();
|
||||
static_transform.header.frame_id = map_frame;
|
||||
static_transform.child_frame_id = "/goal_pose";
|
||||
static_transform.transform = transform.transform;
|
||||
static_broadcaster.sendTransform(static_transform);
|
||||
|
||||
// 关键：四元数转欧拉角计算yaw
|
||||
tf2::Quaternion q(
|
||||
transform.transform.rotation.x,
|
||||
transform.transform.rotation.y,
|
||||
transform.transform.rotation.z,
|
||||
transform.transform.rotation.w
|
||||
);
|
||||
|
||||
double roll, pitch, yaw;
|
||||
tf2::Matrix3x3(q).getRPY(roll, pitch, yaw);
|
||||
|
||||
// 初始化话题发布器
|
||||
ros::Publisher pose_pub = nh.advertise<fast_lio_localization::GoalPose>("/goal_pose", 10);
|
||||
|
||||
// 构建PoseStamped消息
|
||||
fast_lio_localization::GoalPose pose_msg;
|
||||
pose_msg.x = transform.transform.translation.x;
|
||||
pose_msg.y = transform.transform.translation.y;
|
||||
pose_msg.angle = yaw; // 使用正确计算的yaw值
|
||||
pose_msg.max_speed = 4.0;
|
||||
pose_msg.circle = 0.0;
|
||||
pose_msg.tolerance = 0.0;
|
||||
|
||||
ROS_INFO("发布目标位姿: x=%.2f, y=%.2f, yaw=%.2f rad (%.1f°)",
|
||||
pose_msg.x, pose_msg.y, yaw, yaw * 180/M_PI);
|
||||
|
||||
// 持续发布话题消息
|
||||
ros::Rate rate(10); // 10Hz
|
||||
while (ros::ok()) {
|
||||
pose_pub.publish(pose_msg);
|
||||
rate.sleep();
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
87
src/FAST_LIO_LOCALIZATION/src/tor1_serial.cpp
Executable file
87
src/FAST_LIO_LOCALIZATION/src/tor1_serial.cpp
Executable file
@ -0,0 +1,87 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <serial/serial.h>
|
||||
#include <ros/time.h>
|
||||
#include <fast_lio_localization/position.h>
|
||||
|
||||
static serial::Serial ser;
|
||||
static std::mutex serial_mutex;
|
||||
|
||||
// 串口参数
|
||||
void serialInit() {
|
||||
std::lock_guard<std::mutex> lock(serial_mutex);
|
||||
if (ser.isOpen()) ser.close();
|
||||
serial::Timeout to = serial::Timeout::simpleTimeout(100);
|
||||
ser.setPort("/dev/tor1");
|
||||
ser.setBaudrate(115200);
|
||||
ser.setTimeout(to);
|
||||
ser.open();
|
||||
if (ser.isOpen()) {
|
||||
ser.flushInput();
|
||||
ser.flushOutput();
|
||||
ROS_INFO("Serial port initialized and buffers cleared.");
|
||||
} else {
|
||||
ROS_ERROR("Failed to open serial port!tor1");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool read2float(bool &passball, double &x, double &y) {
|
||||
std::lock_guard<std::mutex> lock(serial_mutex);
|
||||
try {
|
||||
// 一帧总长度=帧头1+标志1+数据8+帧尾1=11字节
|
||||
std::string buf = ser.read(12);
|
||||
std::cout << "buf(hex): ";
|
||||
for (size_t i = 0; i < buf.size(); ++i) {
|
||||
printf("%02X ", static_cast<uint8_t>(buf[i]));
|
||||
}
|
||||
std::cout << std::endl;
|
||||
if (buf.size() != 12) return false;
|
||||
if ((uint8_t)buf[0] != 0xFC || (uint8_t)buf[1] != 0x01 || (uint8_t)buf[11] != 0xFD)
|
||||
return false;
|
||||
for (size_t i = 0; i < buf.size(); ++i) {
|
||||
printf("%02X ", static_cast<uint8_t>(buf[i]));
|
||||
}
|
||||
std::cout << std::endl;
|
||||
bool passball_f;
|
||||
float x_f, y_f;
|
||||
memcpy(&passball_f, &buf[2], 1);
|
||||
memcpy(&x_f, &buf[3], 4);
|
||||
memcpy(&y_f, &buf[7], 4);
|
||||
passball = bool(passball_f);
|
||||
x=double(x_f);
|
||||
y=double(y_f);
|
||||
return true;
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
fast_lio_localization::position sub_position;
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "tor1_serial_node");
|
||||
ros::NodeHandle nh;
|
||||
serialInit();
|
||||
double x, y;
|
||||
bool passball;
|
||||
ros::Publisher pub_pos = nh.advertise<fast_lio_localization::position>("/position", 1000);
|
||||
while (ros::ok()) {
|
||||
|
||||
if (read2float(passball ,x , y)) {
|
||||
sub_position.passball = passball;
|
||||
sub_position.x = x;
|
||||
sub_position.y = y;
|
||||
pub_pos.publish(sub_position);
|
||||
ROS_INFO("pass: %d,Dian: %f, Passball: %f",int(passball),x, y);
|
||||
} else {
|
||||
sub_position.passball = false;
|
||||
sub_position.x = 0.0;
|
||||
sub_position.y = 0.0;
|
||||
pub_pos.publish(sub_position);
|
||||
ROS_INFO("Failed to read data from serial_2 port.");
|
||||
}
|
||||
}
|
||||
}
|
||||
180
src/FAST_LIO_LOCALIZATION/src/uart.cpp
Executable file
180
src/FAST_LIO_LOCALIZATION/src/uart.cpp
Executable file
@ -0,0 +1,180 @@
|
||||
#include <ros/ros.h>
|
||||
#include <std_msgs/Float32.h>
|
||||
#include <std_msgs/Float64MultiArray.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <fcntl.h> // 文件控制定义
|
||||
#include <unistd.h> // UNIX标准函数定义
|
||||
#include <termios.h> // POSIX终端控制定义
|
||||
#include <cstring> // 字符串操作函数
|
||||
#include <fast_lio_localization/GoalPose.h>
|
||||
#include <fast_lio_localization/DataAI.h>
|
||||
#include <fast_lio_localization/DataNav.h>
|
||||
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
|
||||
#include <geometry_msgs/Twist.h>
|
||||
class SerialNode
|
||||
{
|
||||
public:
|
||||
SerialNode()
|
||||
{
|
||||
ros::NodeHandle nh;
|
||||
offsets_sub_ = nh.subscribe("/chassis/data_ai", 1000, &SerialNode::xyyawCallback, this);
|
||||
|
||||
// 打开串口
|
||||
serial_fd_ = openSerialPort("/dev/r2self", B115200);
|
||||
if (serial_fd_ == -1) {
|
||||
ROS_ERROR("Failed to open serial port!");
|
||||
}
|
||||
}
|
||||
|
||||
~SerialNode()
|
||||
{
|
||||
if (serial_fd_ != -1) {
|
||||
close(serial_fd_);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
int openSerialPort(const char* port, int baudRate)
|
||||
{
|
||||
// 打开串口设备
|
||||
int fd = open(port, O_RDWR | O_NOCTTY | O_NDELAY);
|
||||
if (fd == -1) {
|
||||
ROS_ERROR_STREAM("Unable to open serial port: " << port);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 配置串口参数
|
||||
struct termios tty;
|
||||
memset(&tty, 0, sizeof(tty));
|
||||
|
||||
// 获取当前串口配置
|
||||
if (tcgetattr(fd, &tty) != 0) {
|
||||
ROS_ERROR_STREAM("Failed to get serial port attributes");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
// 设置波特率
|
||||
cfsetospeed(&tty, baudRate);
|
||||
cfsetispeed(&tty, baudRate);
|
||||
|
||||
// 设置数据位为8位
|
||||
tty.c_cflag &= ~CSIZE;
|
||||
tty.c_cflag |= CS8;
|
||||
|
||||
// 关闭奇偶校验
|
||||
tty.c_cflag &= ~PARENB;
|
||||
|
||||
// 设置停止位为1位
|
||||
tty.c_cflag &= ~CSTOPB;
|
||||
|
||||
// 禁用硬件流控制
|
||||
tty.c_cflag &= ~CRTSCTS;
|
||||
|
||||
// 启用接收和本地模式
|
||||
tty.c_cflag |= CREAD | CLOCAL;
|
||||
|
||||
// 禁用规范输入处理
|
||||
tty.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
|
||||
|
||||
// 禁用软件流控制
|
||||
tty.c_iflag &= ~(IXON | IXOFF | IXANY);
|
||||
|
||||
// 禁用输出处理
|
||||
tty.c_oflag &= ~OPOST;
|
||||
|
||||
// 设置最小字符数和超时时间
|
||||
tty.c_cc[VMIN] = 1; // 至少读取1个字符
|
||||
tty.c_cc[VTIME] = 10; // 超时时间为1秒
|
||||
|
||||
// 应用配置
|
||||
if (tcsetattr(fd, TCSANOW, &tty) != 0) {
|
||||
ROS_ERROR_STREAM("Failed to set serial port attributes");
|
||||
close(fd);
|
||||
return -1;
|
||||
}
|
||||
|
||||
return fd;
|
||||
}
|
||||
|
||||
void xyyawCallback(const fast_lio_localization::DataAI::ConstPtr& msg)
|
||||
{
|
||||
// 提取第一个目标的偏移量
|
||||
float sent_x = static_cast<float>(msg->vx);
|
||||
float sent_y = static_cast<float>(msg->vy);
|
||||
float sent_yaw = static_cast<float>(msg->yaw);
|
||||
float pos = static_cast<float>(msg->pos);
|
||||
float ang = static_cast<float>(msg->ang);
|
||||
bool reach = static_cast<bool>(msg->reach);
|
||||
|
||||
// 立即发送数据包
|
||||
sendDataPacket(-sent_x, -sent_y, sent_yaw, pos, ang, reach);
|
||||
|
||||
}
|
||||
|
||||
void sendDataPacket(float sent_x, float sent_y,float sent_yaw,float pos, float ang , bool reach)
|
||||
{
|
||||
ROS_INFO("Sending data packet...");
|
||||
if (serial_fd_ == -1) {
|
||||
ROS_ERROR("Serial port not open!");
|
||||
return;
|
||||
}
|
||||
|
||||
const uint8_t start_flag = 0xFF;
|
||||
const uint8_t control_frame = 0x09;
|
||||
const uint8_t camera_frame = 0x01;
|
||||
const uint8_t end_flag = 0xFE;
|
||||
|
||||
// 转换为字节数组
|
||||
uint8_t sent_x_bytes[4];
|
||||
uint8_t sent_y_bytes[4];
|
||||
uint8_t sent_yaw_bytes[4];
|
||||
uint8_t pos_bytes[4];
|
||||
uint8_t ang_bytes[4];
|
||||
uint8_t reach_bytes[1];
|
||||
std::memcpy(sent_x_bytes, &sent_x, sizeof(sent_x));
|
||||
std::memcpy(sent_y_bytes, &sent_y, sizeof(sent_y));
|
||||
std::memcpy(sent_yaw_bytes, &sent_yaw, sizeof(sent_yaw));
|
||||
std::memcpy(pos_bytes, &pos, sizeof(pos));
|
||||
std::memcpy(ang_bytes, &ang, sizeof(ang));
|
||||
std::memcpy(reach_bytes, &reach, sizeof(reach));
|
||||
|
||||
|
||||
// 构建数据包
|
||||
std::vector<uint8_t> data_packet;
|
||||
data_packet.reserve(25); // 预分配空间优化性能
|
||||
data_packet.push_back(start_flag);
|
||||
data_packet.push_back(control_frame);
|
||||
data_packet.push_back(camera_frame);
|
||||
data_packet.insert(data_packet.end(), sent_x_bytes, sent_x_bytes + 4);
|
||||
data_packet.insert(data_packet.end(), sent_y_bytes, sent_y_bytes + 4);
|
||||
data_packet.insert(data_packet.end(), sent_yaw_bytes, sent_yaw_bytes + 4);
|
||||
data_packet.insert(data_packet.end(), pos_bytes, pos_bytes + 4);
|
||||
data_packet.insert(data_packet.end(), ang_bytes, ang_bytes + 4);
|
||||
data_packet.insert(data_packet.end(), reach_bytes, reach_bytes + 1);
|
||||
data_packet.push_back(end_flag);
|
||||
|
||||
// 发送数据
|
||||
write(serial_fd_, data_packet.data(), data_packet.size());
|
||||
|
||||
//ROS_INFO("Received offsets: x=%.5f, y=%.5f, yaw=%.5f", sent_x, sent_y, sent_yaw, pos, ang, reach);
|
||||
// 调试输出
|
||||
//ROS_DEBUG("Sent packet: ");
|
||||
for (auto byte : data_packet) {
|
||||
ROS_DEBUG("%02X ", byte);
|
||||
}
|
||||
}
|
||||
|
||||
ros::Subscriber offsets_sub_;
|
||||
int serial_fd_; // 串口文件描述符
|
||||
};
|
||||
|
||||
int main(int argc, char **argv)
|
||||
{
|
||||
ros::init(argc, argv, "serial_node");
|
||||
SerialNode node;
|
||||
ros::spin();
|
||||
return 0;
|
||||
}
|
||||
74
src/FAST_LIO_LOCALIZATION/src/underpan_serial.cpp
Executable file
74
src/FAST_LIO_LOCALIZATION/src/underpan_serial.cpp
Executable file
@ -0,0 +1,74 @@
|
||||
#include <ros/ros.h>
|
||||
#include <tf2/utils.h>
|
||||
#include <std_msgs/Bool.h>
|
||||
#include <serial/serial.h>
|
||||
#include <ros/time.h>
|
||||
#include <fast_lio_localization/underpan.h>
|
||||
|
||||
static serial::Serial ser;
|
||||
static std::mutex serial_mutex;
|
||||
|
||||
// 串口参数
|
||||
void serialInit() {
|
||||
std::lock_guard<std::mutex> lock(serial_mutex);
|
||||
if (ser.isOpen()) ser.close();
|
||||
serial::Timeout to = serial::Timeout::simpleTimeout(100);
|
||||
ser.setPort("/dev/r2self");
|
||||
ser.setBaudrate(115200);
|
||||
ser.setTimeout(to);
|
||||
ser.open();
|
||||
if (ser.isOpen()) {
|
||||
ser.flushInput();
|
||||
ser.flushOutput();
|
||||
ROS_INFO("Serial port initialized and buffers cleared.");
|
||||
} else {
|
||||
ROS_ERROR("Failed to open serial port! underpan");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool read5float(bool &dian) {
|
||||
std::lock_guard<std::mutex> lock(serial_mutex);
|
||||
if (!ser.isOpen()) return false;
|
||||
try {
|
||||
// 一帧总长度=帧头1+标志1+数据8+帧尾1=11字节
|
||||
std::string buf = ser.read(7);
|
||||
// std::cout << "buf(hex): ";
|
||||
// for (size_t i = 0; i < buf.size(); ++i) {
|
||||
// printf("%02X ", static_cast<uint8_t>(buf[i]));
|
||||
// }
|
||||
// std::cout << std::endl;
|
||||
if (buf.size() != 7) return false;
|
||||
if ((uint8_t)buf[0] != 0xFF || (uint8_t)buf[1] != 0x01 || (uint8_t)buf[6] != 0xFE)
|
||||
return false;
|
||||
float dian_f;
|
||||
memcpy(&dian_f, &buf[2], 4);
|
||||
dian =bool(dian_f);
|
||||
return true;
|
||||
} catch (...) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
ros::init(argc, argv, "underpan_serial_node");
|
||||
ros::NodeHandle nh;
|
||||
serialInit();
|
||||
ros::Publisher pub_underpan = nh.advertise<fast_lio_localization::underpan>("/underpan", 1000);
|
||||
fast_lio_localization::underpan underpan_msg;
|
||||
ros::Rate loop_rate(100); //
|
||||
bool dian = false, passball = false;
|
||||
while (ros::ok()) {
|
||||
if (read5float(dian)) {
|
||||
std_msgs::Bool dian_msg, passball_msg;
|
||||
underpan_msg.dian = dian;
|
||||
underpan_msg.passball = passball;
|
||||
pub_underpan.publish(underpan_msg);
|
||||
ROS_INFO("Dian: %d, Passball: %d", dian, passball);
|
||||
} else {
|
||||
// ROS_INFO("Failed to read data from serial port.");
|
||||
}
|
||||
loop_rate.sleep();
|
||||
}
|
||||
}
|
||||
1
src/Livox-SDK2
Submodule
1
src/Livox-SDK2
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit 6a940156dd7151c3ab6a52442d86bc83613bd11b
|
||||
1
src/Point-LIO
Submodule
1
src/Point-LIO
Submodule
@ -0,0 +1 @@
|
||||
Subproject commit 97b0042e397ec71c327966abebff9b4b7e2c154a
|
||||
@ -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
|
||||
}
|
||||
}
|
||||
]
|
||||
|
||||
@ -1,5 +1,4 @@
|
||||
<launch>
|
||||
|
||||
<!--user configure parameters for ros start-->
|
||||
<arg name="lvx_file_path" default="livox_test.lvx"/>
|
||||
<arg name="bd_list" default="100000000000000"/>
|
||||
@ -30,11 +29,12 @@
|
||||
|
||||
<node name="livox_lidar_publisher2" pkg="livox_ros_driver2"
|
||||
type="livox_ros_driver2_node" required="true"
|
||||
output="screen" args="$(arg cmdline_arg)"/>
|
||||
args="$(arg cmdline_arg)"
|
||||
output="screen"/>
|
||||
|
||||
<group if="$(arg rosbag_enable)">
|
||||
<node pkg="rosbag" type="record" name="record" output="screen"
|
||||
args="-a"/>
|
||||
<node pkg="rosbag" type="record" name="record"
|
||||
args="-a" output="screen"/>
|
||||
</group>
|
||||
|
||||
</launch>
|
||||
</launch>
|
||||
@ -2,11 +2,11 @@
|
||||
<launch>
|
||||
<node pkg="pcd2pgm" name="pcd2pgm" type="pcd2pgm" output="screen">
|
||||
<!-- 存放pcd文件的路径-->
|
||||
<param name="file_directory" value= "/home/cmrt/ws_livox/src/FAST_LIO_LOCALIZATION/PCD/" />
|
||||
<param name="file_directory" value= "/home/robofish/ws_lida/src/Point-LIO/PCD/" />i
|
||||
<!-- pcd文件名称-->
|
||||
<param name="file_name" value= "scans" />
|
||||
<param name="file_name" value= "test" />
|
||||
<!-- 选取的范围 最小的高度-->
|
||||
<param name="thre_z_min" value= "0.1" />
|
||||
<param name="thre_z_min" value= "-0.7" />
|
||||
<!-- 选取的范围 最大的高度-->
|
||||
<param name="thre_z_max" value= "1.5" />
|
||||
<!--0 选取高度范围内的，１选取高度范围外的-->
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user