From 9074776d2bc9e4d1fcccdbdb6ae27ee9fa701667 Mon Sep 17 00:00:00 2001 From: zhanli <719901725@qq.com> Date: Wed, 13 Dec 2023 00:55:30 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E9=83=A8=E5=88=86=E6=B3=A8?= =?UTF-8?q?=E9=87=8A=E5=92=8C=E6=96=87=E6=A1=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Code/RK3588/README.md | 4 +- .../.catkin_workspace | 0 .../.gitignore | 0 .../Image/main.png | Bin .../README.md | 0 .../dataset/data1.txt | 0 .../dataset/data2.txt | 0 .../devel/.built_by | 0 .../devel/.catkin | 0 .../devel/.rosinstall | 0 .../devel/_setup_util.py | 0 .../devel/cmake.lock | 0 .../devel/env.sh | 0 .../devel/local_setup.bash | 0 .../devel/local_setup.sh | 0 .../devel/local_setup.zsh | 0 .../devel/setup.bash | 0 .../devel/setup.sh | 0 .../devel/setup.zsh | 0 .../src/.idea/.gitignore | 0 .../src/.idea/deployment.xml | 0 .../src/.idea/misc.xml | 0 .../src/.idea/modules.xml | 0 .../src/.idea/src.iml | 0 .../src/.idea/vcs.xml | 0 .../src/CMakeLists.txt | 0 .../src/ros_merge_test/CMakeLists.txt | 0 .../src/ros_merge_test/Map/01.png | Bin .../src/ros_merge_test/Map/02.png | Bin .../src/ros_merge_test/Map/03.png | Bin .../src/ros_merge_test/Map/04.png | Bin .../src/ros_merge_test/Map/05.png | Bin .../src/ros_merge_test/Map/06.png | Bin .../src/ros_merge_test/Map/07.png | Bin .../src/ros_merge_test/Map/08.png | Bin .../src/ros_merge_test/Map/09.png | Bin .../src/ros_merge_test/Map/10.png | Bin .../src/ros_merge_test/Map/11.png | Bin .../src/ros_merge_test/Map/12.png | Bin .../src/ros_merge_test/Map/13.png | Bin .../src/ros_merge_test/Map/output_image.png | Bin .../src/ros_merge_test/include/align.h | 2 +- .../src/ros_merge_test/include/avoid.h | 0 .../src/ros_merge_test/include/mapping.h | 0 .../ros_merge_test/include/read_sensor_data.h | 4 +- .../src/ros_merge_test/include/senddata.h | 29 ++++++ .../src/ros_merge_test/include/system.h | 8 +- .../src/ros_merge_test/include/type.h | 11 +-- .../src/ros_merge_test/include/uwb.h | 2 +- .../src/ros_merge_test/msg/RawImu.msg | 0 .../src/ros_merge_test/package.xml | 0 .../src/ros_merge_test/src/align.cpp | 17 ++++ .../src/ros_merge_test/src/main.cpp | 71 +++++++++++++++ .../src/ros_merge_test/src/mapping.cpp | 28 +++--- .../ros_merge_test/src/read_sensor_data.cpp | 13 +-- .../src/ros_merge_test/src/senddata.cpp | 83 ++++++++++++++++++ .../src/ros_merge_test/src/system.cpp | 3 - .../src/ros_merge_test/src/uwb.cpp | 72 ++++----------- .../src/ros_merge_test/include/senddata.h | 37 -------- .../src/ros_merge_test/src/main.cpp | 62 ------------- .../src/ros_merge_test/src/senddata.cpp | 63 ------------- 61 files changed, 254 insertions(+), 255 deletions(-) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/.catkin_workspace (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/.gitignore (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/Image/main.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/README.md (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/dataset/data1.txt (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/dataset/data2.txt (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/.built_by (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/.catkin (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/.rosinstall (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/_setup_util.py (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/cmake.lock (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/env.sh (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/local_setup.bash (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/local_setup.sh (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/local_setup.zsh (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/setup.bash (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/setup.sh (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/devel/setup.zsh (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/.idea/.gitignore (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/.idea/deployment.xml (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/.idea/misc.xml (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/.idea/modules.xml (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/.idea/src.iml (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/.idea/vcs.xml (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/CMakeLists.txt (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/CMakeLists.txt (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/01.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/02.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/03.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/04.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/05.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/06.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/07.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/08.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/09.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/10.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/11.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/12.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/13.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/Map/output_image.png (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/include/align.h (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/include/avoid.h (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/include/mapping.h (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/include/read_sensor_data.h (92%) create mode 100644 Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/senddata.h rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/include/system.h (66%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/include/type.h (74%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/include/uwb.h (97%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/msg/RawImu.msg (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/package.xml (100%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/src/align.cpp (89%) create mode 100644 Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/main.cpp rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/src/mapping.cpp (86%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/src/read_sensor_data.cpp (69%) create mode 100644 Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/senddata.cpp rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/src/system.cpp (93%) rename Code/RK3588/{Robot_ROS_Driver => Robot_ROS_APP}/src/ros_merge_test/src/uwb.cpp (60%) delete mode 100644 Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/senddata.h delete mode 100644 Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/main.cpp delete mode 100644 Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/senddata.cpp diff --git a/Code/RK3588/README.md b/Code/RK3588/README.md index 62820ab..8d793d0 100644 --- a/Code/RK3588/README.md +++ b/Code/RK3588/README.md @@ -1,7 +1,9 @@ # 目录说明 +更新：2023-12-13 詹力 Review + 1. `PIBot_ROS` : ROS小车的ROS上层支持包负责`IMU`和`ODM`等数据的读取 -2. `Robot_ROS_Driver` : 定位融合代码 +2. `Robot_ROS_APP` : 定位融合代码 其他资料： diff --git a/Code/RK3588/Robot_ROS_Driver/.catkin_workspace b/Code/RK3588/Robot_ROS_APP/.catkin_workspace similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/.catkin_workspace rename to Code/RK3588/Robot_ROS_APP/.catkin_workspace diff --git a/Code/RK3588/Robot_ROS_Driver/.gitignore b/Code/RK3588/Robot_ROS_APP/.gitignore similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/.gitignore rename to Code/RK3588/Robot_ROS_APP/.gitignore diff --git a/Code/RK3588/Robot_ROS_Driver/Image/main.png b/Code/RK3588/Robot_ROS_APP/Image/main.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/Image/main.png rename to Code/RK3588/Robot_ROS_APP/Image/main.png diff --git a/Code/RK3588/Robot_ROS_Driver/README.md b/Code/RK3588/Robot_ROS_APP/README.md similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/README.md rename to Code/RK3588/Robot_ROS_APP/README.md diff --git a/Code/RK3588/Robot_ROS_Driver/dataset/data1.txt b/Code/RK3588/Robot_ROS_APP/dataset/data1.txt similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/dataset/data1.txt rename to Code/RK3588/Robot_ROS_APP/dataset/data1.txt diff --git a/Code/RK3588/Robot_ROS_Driver/dataset/data2.txt b/Code/RK3588/Robot_ROS_APP/dataset/data2.txt similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/dataset/data2.txt rename to Code/RK3588/Robot_ROS_APP/dataset/data2.txt diff --git a/Code/RK3588/Robot_ROS_Driver/devel/.built_by b/Code/RK3588/Robot_ROS_APP/devel/.built_by similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/.built_by rename to Code/RK3588/Robot_ROS_APP/devel/.built_by diff --git a/Code/RK3588/Robot_ROS_Driver/devel/.catkin b/Code/RK3588/Robot_ROS_APP/devel/.catkin similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/.catkin rename to Code/RK3588/Robot_ROS_APP/devel/.catkin diff --git a/Code/RK3588/Robot_ROS_Driver/devel/.rosinstall b/Code/RK3588/Robot_ROS_APP/devel/.rosinstall similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/.rosinstall rename to Code/RK3588/Robot_ROS_APP/devel/.rosinstall diff --git a/Code/RK3588/Robot_ROS_Driver/devel/_setup_util.py b/Code/RK3588/Robot_ROS_APP/devel/_setup_util.py similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/_setup_util.py rename to Code/RK3588/Robot_ROS_APP/devel/_setup_util.py diff --git a/Code/RK3588/Robot_ROS_Driver/devel/cmake.lock b/Code/RK3588/Robot_ROS_APP/devel/cmake.lock similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/cmake.lock rename to Code/RK3588/Robot_ROS_APP/devel/cmake.lock diff --git a/Code/RK3588/Robot_ROS_Driver/devel/env.sh b/Code/RK3588/Robot_ROS_APP/devel/env.sh similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/env.sh rename to Code/RK3588/Robot_ROS_APP/devel/env.sh diff --git a/Code/RK3588/Robot_ROS_Driver/devel/local_setup.bash b/Code/RK3588/Robot_ROS_APP/devel/local_setup.bash similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/local_setup.bash rename to Code/RK3588/Robot_ROS_APP/devel/local_setup.bash diff --git a/Code/RK3588/Robot_ROS_Driver/devel/local_setup.sh b/Code/RK3588/Robot_ROS_APP/devel/local_setup.sh similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/local_setup.sh rename to Code/RK3588/Robot_ROS_APP/devel/local_setup.sh diff --git a/Code/RK3588/Robot_ROS_Driver/devel/local_setup.zsh b/Code/RK3588/Robot_ROS_APP/devel/local_setup.zsh similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/local_setup.zsh rename to Code/RK3588/Robot_ROS_APP/devel/local_setup.zsh diff --git a/Code/RK3588/Robot_ROS_Driver/devel/setup.bash b/Code/RK3588/Robot_ROS_APP/devel/setup.bash similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/setup.bash rename to Code/RK3588/Robot_ROS_APP/devel/setup.bash diff --git a/Code/RK3588/Robot_ROS_Driver/devel/setup.sh b/Code/RK3588/Robot_ROS_APP/devel/setup.sh similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/setup.sh rename to Code/RK3588/Robot_ROS_APP/devel/setup.sh diff --git a/Code/RK3588/Robot_ROS_Driver/devel/setup.zsh b/Code/RK3588/Robot_ROS_APP/devel/setup.zsh similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/devel/setup.zsh rename to Code/RK3588/Robot_ROS_APP/devel/setup.zsh diff --git a/Code/RK3588/Robot_ROS_Driver/src/.idea/.gitignore b/Code/RK3588/Robot_ROS_APP/src/.idea/.gitignore similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/.idea/.gitignore rename to Code/RK3588/Robot_ROS_APP/src/.idea/.gitignore diff --git a/Code/RK3588/Robot_ROS_Driver/src/.idea/deployment.xml b/Code/RK3588/Robot_ROS_APP/src/.idea/deployment.xml similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/.idea/deployment.xml rename to Code/RK3588/Robot_ROS_APP/src/.idea/deployment.xml diff --git a/Code/RK3588/Robot_ROS_Driver/src/.idea/misc.xml b/Code/RK3588/Robot_ROS_APP/src/.idea/misc.xml similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/.idea/misc.xml rename to Code/RK3588/Robot_ROS_APP/src/.idea/misc.xml diff --git a/Code/RK3588/Robot_ROS_Driver/src/.idea/modules.xml b/Code/RK3588/Robot_ROS_APP/src/.idea/modules.xml similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/.idea/modules.xml rename to Code/RK3588/Robot_ROS_APP/src/.idea/modules.xml diff --git a/Code/RK3588/Robot_ROS_Driver/src/.idea/src.iml b/Code/RK3588/Robot_ROS_APP/src/.idea/src.iml similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/.idea/src.iml rename to Code/RK3588/Robot_ROS_APP/src/.idea/src.iml diff --git a/Code/RK3588/Robot_ROS_Driver/src/.idea/vcs.xml b/Code/RK3588/Robot_ROS_APP/src/.idea/vcs.xml similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/.idea/vcs.xml rename to Code/RK3588/Robot_ROS_APP/src/.idea/vcs.xml diff --git a/Code/RK3588/Robot_ROS_Driver/src/CMakeLists.txt b/Code/RK3588/Robot_ROS_APP/src/CMakeLists.txt similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/CMakeLists.txt rename to Code/RK3588/Robot_ROS_APP/src/CMakeLists.txt diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/CMakeLists.txt b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/CMakeLists.txt similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/CMakeLists.txt rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/CMakeLists.txt diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/01.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/01.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/01.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/01.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/02.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/02.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/02.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/02.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/03.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/03.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/03.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/03.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/04.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/04.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/04.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/04.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/05.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/05.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/05.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/05.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/06.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/06.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/06.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/06.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/07.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/07.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/07.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/07.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/08.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/08.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/08.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/08.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/09.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/09.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/09.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/09.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/10.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/10.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/10.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/10.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/11.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/11.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/11.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/11.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/12.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/12.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/12.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/12.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/13.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/13.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/13.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/13.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/output_image.png b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/output_image.png similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/Map/output_image.png rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/Map/output_image.png diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/align.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/align.h similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/align.h rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/align.h index 860f32f..c75e0f2 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/align.h +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/align.h @@ -26,6 +26,7 @@ namespace uwb_slam{ ros::Subscriber wheel_odom_sub_; ros::Subscriber imu_sub_; ros::Subscriber odom_sub_; + Imu_odom_pose_data imu_odom_; Uwb_data uwb_data_; ros::Time tmp ; @@ -35,7 +36,6 @@ namespace uwb_slam{ cv::Mat img1; std::queue> data_queue; std::shared_ptr uwb_; - }; }; #endif diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/avoid.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/avoid.h similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/avoid.h rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/avoid.h diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/mapping.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/mapping.h similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/mapping.h rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/mapping.h diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/read_sensor_data.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/read_sensor_data.h similarity index 92% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/read_sensor_data.h rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/read_sensor_data.h index 61a7df3..39c7430 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/read_sensor_data.h +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/read_sensor_data.h @@ -15,10 +15,10 @@ namespace uwb_slam{ typedef boost::shared_ptr OdomConstPtr; typedef boost::shared_ptr ImuConstPtr; - class Read_sensor_data + class ReadSensorData { public: - Read_sensor_data(); + ReadSensorData(); void Run(int argc, char* argv[]); //void set_uwb(Uwb * uwb); diff --git a/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/senddata.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/senddata.h new file mode 100644 index 0000000..ff8f9bc --- /dev/null +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/senddata.h @@ -0,0 +1,29 @@ +#include +#include +#include +#include +#include +#include +#include "uwb.h" +#ifndef SENDDATA_H +#define SENDDATA_H + +namespace uwb_slam{ +class Senddata{ + public: + Senddata(){}; + void publishOdometry( std::shared_ptruwb); + void Run(std::shared_ptruwb); + void odomCB(const nav_msgs::Odometry& odom); + + std::mutex mMutexSend; + private: + ros::Publisher position_pub_; + ros::Subscriber odom_sub_; + ros::NodeHandle nh_; + nav_msgs::Odometry odom_;//odom的消息类型 + nav_msgs::Odometry sub_odom_;//odom的消息类型 +}; +} + +#endif \ No newline at end of file diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/system.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/system.h similarity index 66% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/system.h rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/system.h index 9495710..b0c5f85 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/system.h +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/system.h @@ -18,10 +18,10 @@ namespace uwb_slam{ void Run(); public: - std::shared_ptrMapping_; - std::shared_ptrUwb_; - std::shared_ptrSender_; - std::shared_ptrAlign_; + std::shared_ptr Mapping_; + std::shared_ptr Uwb_; + std::shared_ptr Sender_; + std::shared_ptr Align_; // Uwb* Uwb_ ; // Senddata* Sender_; diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/type.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/type.h similarity index 74% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/type.h rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/type.h index 8d28092..d92ef27 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/type.h +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/type.h @@ -18,13 +18,14 @@ struct Imu_data struct Imu_odom_pose_data { - Imu_data imu_data_; - double pose_[3]; - double quat_[4]; + Imu_data imu_data_; + double pose_[3]; /* 位置信息 */ + double quat_[4]; /* 姿态信息(四元数) */ double vxy_; double angle_v_; Imu_odom_pose_data(){}; - Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){}; + Imu_odom_pose_data( Imu_data imu_data,double x,double y,double z, double qw, double qx, + double qy, double qz,double vxy, double angle_v):imu_data_(imu_data),pose_{x,y,z},quat_{qw,qx,qy,qz},vxy_(vxy),angle_v_(angle_v){}; }; /*struct Odom_data @@ -43,7 +44,7 @@ struct Imu_odom_pose_data }; */ -struct Uwb_data +struct Uwb_data { float x_,y_; ros::Time uwb_t_; diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/uwb.h b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/uwb.h similarity index 97% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/uwb.h rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/uwb.h index 911b97e..22fefb9 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/uwb.h +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/include/uwb.h @@ -20,7 +20,7 @@ namespace uwb_slam{ void Run(); bool checknewdata(); void feed_imu_odom_pose_data(); - void Serread(); + void UartUSBRead(); diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/msg/RawImu.msg b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/msg/RawImu.msg similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/msg/RawImu.msg rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/msg/RawImu.msg diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/package.xml b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/package.xml similarity index 100% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/package.xml rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/package.xml diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/align.cpp b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/align.cpp similarity index 89% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/align.cpp rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/align.cpp index a370d59..6bba774 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/align.cpp +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/align.cpp @@ -1,11 +1,20 @@ +/******************** (C) COPYRIGHT 2023 UPBot ********************************** +* File Name : align.cpp +* Current Version : V1.0 +* Date of Issued : 2023.12.13 zhanli@review +* Comments : 传感器数据对齐 +********************************************************************************/ #include "align.h" namespace uwb_slam{ + void Align::Run() { tmp = ros::Time::now(); ros::Time tmp1 = ros::Time::now(); ros::Time tmp2 = ros::Time::now(); + + wheel_odom_sub_= nh_.subscribe("wheel_odom",10,&Align::wheel_odomCB,this); imu_sub_= nh_.subscribe("raw_imu",10,&Align::imuCB,this); odom_sub_= nh_.subscribe("odom",10,&Align::odomCB,this); @@ -120,12 +129,20 @@ namespace uwb_slam{ return; } + /**--------------------------------------------------------------------- + * Function : odomCB + * Description : 里程计的回调函数, 定期会被ROS调用传参，这个函数不能做过于耗时 + * 的操作 + * Input : nav_msgs::Odometry& odom : 里程计输入结构体 + * Date : 2023/12/13 zhanli@review + *---------------------------------------------------------------------**/ void Align::odomCB(const nav_msgs::Odometry& odom) { odom_tmp_ = odom.header.stamp; imu_odom_.pose_[0] = odom.pose.pose.position.x; imu_odom_.pose_[1] = odom.pose.pose.position.y; imu_odom_.pose_[2] = odom.pose.pose.position.z; + imu_odom_.quat_[0] = odom.pose.pose.orientation.x; imu_odom_.quat_[1] = odom.pose.pose.orientation.y; imu_odom_.quat_[2] = odom.pose.pose.orientation.z; diff --git a/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/main.cpp b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/main.cpp new file mode 100644 index 0000000..4d19ca7 --- /dev/null +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/main.cpp @@ -0,0 +1,71 @@ +/******************** (C) COPYRIGHT 2023 UPBot ********************************** +* File Name : main.cpp +* Current Version : V1.0 +* Date of Issued : 2023.12.13 zhanli@review +* Comments : UPbot割草机器人项目传感器融合定位入口函数 +********************************************************************************/ +#include "../include/system.h" +#include "../include/uwb.h" +#include +#include +#include +#include "senddata.h" + + +/**--------------------------------------------------------------------- +* Function : main +* Description : 多传感器融合定位的入口函数 +* Date : 2023/12/13 zhanli@review +*---------------------------------------------------------------------**/ +int main(int argc, char** argv) +{ + // Initialize the ROS node + ros::init(argc, argv, "locate_info_pub_node"); + + std::shared_ptr system = std::make_shared(); + std::shared_ptr mp = std::make_shared(); + std::shared_ptr uwb = std::make_shared(); + std::shared_ptr sender = std::make_shared(); + std::shared_ptr align = std::make_shared(); + + // uwb_slam::System* system = new uwb_slam::System(); + // uwb_slam::Mapping* mp = new uwb_slam::Mapping(); + // uwb_slam::Uwb* uwb = new uwb_slam::Uwb(); + // uwb_slam::Senddata* sender = new uwb_slam::Senddata(); + + system->Mapping_ = mp; + system->Mapping_->uwb_ = uwb; + system->Uwb_ = uwb; + system->Sender_ = sender; + system->Align_ = align; + + mp->uwb_ = system->Uwb_; + align->uwb_ = system->Uwb_; + + + // control data fllow in system + std::thread rose_trd([&system]() { + system->Run(); + }); + // uwb serried read + std::thread uwb_trd([&uwb]() { + uwb->Run(); + }); + + // 建图部分暂时没有使用到 + /*std::thread map_trd([&mp]() { + mp->Run(); + });*/ + + std::thread sender_trd([&sender, uwb]() { + sender->Run(uwb); + }); + + std::thread align_trd([&align]() { + align->Run(); + }); + + // Start the ROS node's main loop + ros::spin(); + //System->run() +} diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/mapping.cpp b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp similarity index 86% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/mapping.cpp rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp index af74e6f..9f9ddc2 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/mapping.cpp +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/mapping.cpp @@ -1,6 +1,6 @@ #include "mapping.h" #include -#include // 包含 usleep() 函数所在的头文件 +#include #include #include @@ -16,9 +16,7 @@ namespace uwb_slam { //std::unique_lock lock(mMutexMap); mv_uwb_point_.push(data); - } - void Mapping::process() { @@ -72,23 +70,26 @@ namespace uwb_slam cv::destroyAllWindows(); } */ - while(1){ + while(1) + { + // 这个地方会持续阻塞 int key = cv::waitKey(0); - if (key == 'q' ) { + if (key == 'q') { read_uwb_ = true; std::cout << "non" << key << std::endl; //cv::destroyAllWindows(); } - while( read_uwb_ )//按下空格键 + + while(read_uwb_ ) // 按下空格键 { int key2 = cv::waitKey(1); if (key2 == 'q' ) { //TODO: save - // std::cout << << std::endl; - std::string pngimage="/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径 - cv::imwrite(pngimage,img); - read_uwb_ = false; - break; + + std::string pngimage = "/home/firefly/Project_Ros/src/ros_merge_test/Map/output_image.png";//保存的图片文件路径 + cv::imwrite(pngimage, img); + read_uwb_ = false; + break; } this->feed_uwb_data(cv::Point2d(uwb_->x ,uwb_->y)); @@ -102,15 +103,10 @@ namespace uwb_slam //std::cout << " start process" << std::endl; process(); //std::cout << " end process" << std::endl; - } } // std::cout << "out" << key << std::endl; - } - - - //std::string pngimage="../Map/pngimage.png";//保存的图片文件路径 //cv::imwrite(pngimage,img); diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/read_sensor_data.cpp b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/read_sensor_data.cpp similarity index 69% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/read_sensor_data.cpp rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/read_sensor_data.cpp index e74d0cd..34bd9ee 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/read_sensor_data.cpp +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/read_sensor_data.cpp @@ -6,24 +6,27 @@ namespace uwb_slam { //void Read_sensor_data::set_uwb(){} - void Read_sensor_data::imu_call_back(const ImuConstPtr& imu){ + void ReadSensorData::imu_call_back(const ImuConstPtr& imu){ Imu_data d1= Imu_data(imu->linear_acceleration.x,imu->linear_acceleration.y,imu->linear_acceleration.z, imu->angular_velocity.x,imu->angular_velocity.y,imu->angular_velocity.z); } - void Read_sensor_data::odom_call_back(const OdomConstPtr& odom){ + void ReadSensorData::odom_call_back(const OdomConstPtr& odom){ Odom_data d1 = Odom_data(odom->pose.pose.position.x, odom->pose.pose.position.y, odom->pose.pose.position.z, odom->pose.pose.orientation.w,odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z, odom->header.stamp,odom->twist.twist.linear.x,odom->twist.twist.linear.y,odom->twist.twist.angular.z); } - void Read_sensor_data::Run(int argc, char* argv[]){ + void ReadSensorData::Run(int argc, char* argv[]){ ros::init(argc, argv, "imu_odom"); // 创建一个节点句柄 ros::NodeHandle nh; - //imu_sub_ = nh.subscribe("imu/data_raw", 1000, this->imu_call_back); - //odom_sub_ =nh.subscribe("odom", 1000,odom_call_back); + + //imu_sub_ = nh.subscribe("imu/data_raw", 1000, this->imu_call_back); + //odom_sub_ = nh.subscribe("odom", 1000, odom_call_back); + + // 运行ROS事件循环 ros::spin(); } diff --git a/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/senddata.cpp b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/senddata.cpp new file mode 100644 index 0000000..5e1e628 --- /dev/null +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/senddata.cpp @@ -0,0 +1,83 @@ +/******************** (C) COPYRIGHT 2023 UPBot ********************************** +* File Name : senddata.cpp +* Current Version : V1.0 +* Date of Issued : 2023.12.13 zhanli@review +* Comments : UWB数据的发送 +********************************************************************************/ +#include "senddata.h" +namespace uwb_slam{ + +/**--------------------------------------------------------------------- +* Function : Senddata::Run +* Description : UWB位置发布以及odm数据的订阅 +* Date : 2023/12/13 zhanli@review +*---------------------------------------------------------------------**/ +void Senddata::Run(std::shared_ptruwb){ + // 初始化了一个名为loop_rate的ros::Rate对象,频率设置为10赫兹 + ros::Rate loop_rate(10); + // 初始化一个ROS发布者，用于发布nav_msgs::Odometry类型的消息 + // 主题被设置为"uwb_odom"，队列大小为50 + position_pub_ = nh_.advertise("uwb_odom", 50); + // 初始化了一个ROS订阅者，用于订阅"odom"主题。它指定了当在该主题上接收到 + // 消息时，将调用Senddata类的odomCB回调函数。队列大小被设置为10 + odom_sub_ = nh_.subscribe("odom", 10, &Senddata::odomCB,this); + + while(ros::ok()){ + // 按照10Hz频率发布uwb信息 + publishOdometry(uwb); + ros::spinOnce(); + // 用于控制循环速率 + loop_rate.sleep(); + } +} + +void Senddata::odomCB(const nav_msgs::Odometry& odom){ + // 这个地方接收的是轮速里程计的信息 + // 包含位置和姿态 + sub_odom_ = odom; + return; +} + +/**--------------------------------------------------------------------- +* Function : Senddata::publishOdometry +* Description : 发布UWB里程计数据，这里读取的数据到底是什么，依旧存在疑问 +* Date : 2023/12/13 zhanli@review +*---------------------------------------------------------------------**/ +void Senddata::publishOdometry(std::shared_ptr uwb) +{ + + std::mutex mMutexSend; + + ros::Time current_time = ros::Time::now(); + + // 设置 Odometry 消息的头部信息 + odom_.header.stamp = current_time; // 这个地方获取的时间是否会存在问题? + odom_.header.frame_id = "odom"; // 设置坐标系为 "map" + odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link" + + // 填充 Odometry 消息的位置信息 + odom_.pose.pose.position.x = uwb->x; + odom_.pose.pose.position.y = uwb->y; + odom_.pose.pose.position.z = 0.0; + + + // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态） + // tf2::Quaternion quat; + // quat.setRPY(0, 0, uwb->theta); + + // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0 + // odom.pose.pose.orientation.x = quat.x(); + // odom.pose.pose.orientation.y = quat.y(); + // odom.pose.pose.orientation.z = quat.z(); + // odom.pose.pose.orientation.w = quat.w(); + + // 从里程计拿到姿态信息 + odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x; + odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y; + odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z; + odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w; + + // 发布 Odometry 消息 + position_pub_.publish(odom_); +} +} // namespace uwb_slam \ No newline at end of file diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/system.cpp b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/system.cpp similarity index 93% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/system.cpp rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/system.cpp index 1787a24..12eb8ad 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/system.cpp +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/system.cpp @@ -5,9 +5,6 @@ namespace uwb_slam{ void System::Run() { while(1){ - } - } - } \ No newline at end of file diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/uwb.cpp b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp similarity index 60% rename from Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/uwb.cpp rename to Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp index 81ce00e..b3cd31e 100644 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/uwb.cpp +++ b/Code/RK3588/Robot_ROS_APP/src/ros_merge_test/src/uwb.cpp @@ -1,3 +1,9 @@ +/******************** (C) COPYRIGHT 2023 UPBot ********************************** +* File Name : uwb.cpp +* Current Version : V1.0 +* Date of Issued : 2023.12.13 zhanli@review +* Comments : UWB数据驱动, 负责从串口读取USB并发布出去 +********************************************************************************/ #include "uwb.h" #include @@ -7,38 +13,29 @@ namespace uwb_slam{ // Uwb::Uwb(){ - //int pre_seq = -1; } void Uwb::Run() { - while(1){ - this->Serread(); - // std::cout<<"s"<x<UartUSBRead(); } - - } - void Uwb::Serread() + void Uwb::UartUSBRead() { try { boost::asio::io_service io; boost::asio::serial_port serial(io, "/dev/ttyUSB0"); // 替换成你的串口设备路径 - serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率 - serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位 - serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位 - serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位 + serial.set_option(boost::asio::serial_port_base::baud_rate(115200)); // 设置波特率 + serial.set_option(boost::asio::serial_port_base::character_size(8)); // 设置数据位 + serial.set_option(boost::asio::serial_port_base::parity(boost::asio::serial_port_base::parity::none)); // 设置校验位 + serial.set_option(boost::asio::serial_port_base::stop_bits(boost::asio::serial_port_base::stop_bits::one)); // 设置停止位 uint8_t tmpdata[12]; - // std::cerr << "befor read" << std::endl; + size_t bytesRead = boost::asio::read(serial, boost::asio::buffer(tmpdata, 12)); // 读取串口数据 // std::cerr << "after read" << std::endl; @@ -66,51 +63,16 @@ namespace uwb_slam{ // pre_seq = static_cast(tmpdata[3]); std::cout << "theta: " << theta << " distance: " << distance << std::endl; - //std::cout std::endl; + } catch (const std::exception& ex) { - std::cerr << "Exception: " << ex.what() << std::endl; + std::cerr << "[ERR]: uwb.cpp::Uart USB read data exception: " + << ex.what() << std::endl; } } - void fusion() { - } - - - - }; -// bool Uwb::checknewdata() -// { -// std::unique_lock lock(mMutexUwb); -// return !v_buffer_imu_odom_pose_data_.empty(); -// } - -// void Uwb::Run() { -// while(1) -// { -// if(checknewdata()) -// { -// { -// std::unique_lock lock(mMutexUwb); -// Imu_odom_pose_data imu_odom_pose_data = v_buffer_imu_odom_pose_data_.front(); -// v_buffer_imu_odom_pose_data_.pop(); -// } - - - -// } -// } - - - -// } - -// void Uwb::feed_imu_odom_pose_data(const Imu_odom_pose_data& imu_odom_pose_data){ -// std::unique_lock lock(mMutexUwb); -// v_buffer_imu_odom_pose_data_.push(imu_odom_pose_data); -// } diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/senddata.h b/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/senddata.h deleted file mode 100644 index efef283..0000000 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/include/senddata.h +++ /dev/null @@ -1,37 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include "uwb.h" -#ifndef SENDDATA_H -#define SENDDATA_H - - -namespace uwb_slam{ - - class Senddata - { - public: - Senddata(){}; - void publishOdometry( std::shared_ptruwb); - void Run(std::shared_ptruwb); - void odomCB(const nav_msgs::Odometry& odom); - - - std::mutex mMutexSend; - private: - ros::Publisher position_pub_; - ros::Subscriber odom_sub_; - ros::NodeHandle nh_; - nav_msgs::Odometry odom_;//odom的消息类型 - nav_msgs::Odometry sub_odom_;//odom的消息类型 - - - - }; - -} - -#endif \ No newline at end of file diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/main.cpp b/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/main.cpp deleted file mode 100644 index 0fce6da..0000000 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/main.cpp +++ /dev/null @@ -1,62 +0,0 @@ -#include "../include/system.h" -#include "../include/uwb.h" - - -#include -#include -#include -#include "senddata.h" - - - -int main(int argc, char** argv) -{ - ros::init(argc, argv, "locate_info_pub_node"); // Initialize the ROS node - - std::shared_ptrsystem = std::make_shared(); - std::shared_ptrmp = std::make_shared(); - std::shared_ptruwb = std::make_shared(); - std::shared_ptrsender = std::make_shared(); - std::shared_ptralign = std::make_shared(); - - // uwb_slam::System* system = new uwb_slam::System(); - // uwb_slam::Mapping* mp = new uwb_slam::Mapping(); - // uwb_slam::Uwb* uwb = new uwb_slam::Uwb(); - // uwb_slam::Senddata* sender = new uwb_slam::Senddata(); - - - system->Mapping_ = mp; - system->Mapping_->uwb_ = uwb; - system->Uwb_ = uwb; - system->Sender_ = sender; - system->Align_ = align; - - - mp->uwb_ = system->Uwb_; - align->uwb_=system->Uwb_; - - - // control data fllow in system - std::thread rose_trd ([&system]() { - system->Run(); - }); - // uwb serried read - std::thread uwb_trd([&uwb]() { - uwb->Run(); - }); - // build map - /*std::thread map_trd ([&mp]() { - mp->Run(); - });*/ - - std::thread sender_trd ([&sender, uwb]() { - sender->Run(uwb); - }); - - std::thread align_trd ([&align]() { - align->Run(); - }); - - ros::spin(); // Start the ROS node's main loop - //System->run() -} diff --git a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/senddata.cpp b/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/senddata.cpp deleted file mode 100644 index a98fe71..0000000 --- a/Code/RK3588/Robot_ROS_Driver/src/ros_merge_test/src/senddata.cpp +++ /dev/null @@ -1,63 +0,0 @@ -#include "senddata.h" - -namespace uwb_slam -{ - void Senddata::Run(std::shared_ptruwb){ - - ros::Rate loop_rate(10); - position_pub_=nh_.advertise("uwb_odom",50); - odom_sub_=nh_.subscribe("odom",10,&Senddata::odomCB,this); - while(ros::ok()){ - publishOdometry(uwb); - ros::spinOnce(); - loop_rate.sleep(); - } - - - } - void Senddata::odomCB(const nav_msgs::Odometry& odom){ - sub_odom_ = odom; - return; - } - - void Senddata::publishOdometry(std::shared_ptruwb) - { - - std::mutex mMutexSend; - ros::Time current_time = ros::Time::now(); - - // 设置 Odometry 消息的头部信息 - odom_.header.stamp = current_time; - odom_.header.frame_id = "odom"; // 设置坐标系为 "map" - odom_.child_frame_id = "base_link"; // 设置坐标系为 "base_link" - - // 填充 Odometry 消息的位置信息 - odom_.pose.pose.position.x = uwb->x; - odom_.pose.pose.position.y = uwb->y; - odom_.pose.pose.position.z = 0.0; - - - // 填充 Odometry 消息的姿态信息（使用四元数来表示姿态） - // tf2::Quaternion quat; - // quat.setRPY(0, 0, uwb->theta); // 设置了 yaw 角度，其他 roll 和 pitch 设置为 0 - // odom.pose.pose.orientation.x = quat.x(); - // odom.pose.pose.orientation.y = quat.y(); - // odom.pose.pose.orientation.z = quat.z(); - // odom.pose.pose.orientation.w = quat.w(); - - odom_.pose.pose.orientation.x = sub_odom_.pose.pose.orientation.x; - odom_.pose.pose.orientation.y = sub_odom_.pose.pose.orientation.y; - odom_.pose.pose.orientation.z = sub_odom_.pose.pose.orientation.z; - odom_.pose.pose.orientation.w = sub_odom_.pose.pose.orientation.w; - - // 发布 Odometry 消息 - position_pub_.publish(odom_); - - - } - - - - - -} // namespace uwb_slam \ No newline at end of file