diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..042e9d7
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,6 @@
+.cache
+build
+devel
+cmake-build-*
+.catkin_workspace
+/src/CMakeLists.txt
diff --git a/README.md b/README.md
index e69de29..d1cd988 100644
--- a/README.md
+++ b/README.md
@@ -0,0 +1,21 @@
+# 光珀项目感知算法
+
+
+## 功能包guangpo_yolo
+
+使用rk3588运行yolov5，订阅图像数据，识别电梯
+再根据识别到的人和电梯，得到电梯的拥挤度
+
+具体拥挤度原理：
+识别电梯和人后，根据高度进行单目深度估计
+根据估计到的深度和相机内参得到相机坐标系下的3d位置
+然后电梯和人距离在一定范围内，可以认定有关联
+
+
+## 功能包image_saver
+订阅图像数据，生成图片，方便录制数据包后标定数据
+
+配置文件在对应包路径下的yaml_config/yaml_config.yaml文件中，可以配置：
+1. 订阅的话题
+2. 间隔多少帧生成一张图片
+3. 生成文件的保存路径
diff --git a/src/image_saver/.gitignore b/src/image_saver/.gitignore
new file mode 100644
index 0000000..15b9005
--- /dev/null
+++ b/src/image_saver/.gitignore
@@ -0,0 +1,2 @@
+/images
+images.*
diff --git a/src/image_saver/CMakeLists.txt b/src/image_saver/CMakeLists.txt
new file mode 100644
index 0000000..b7ce92a
--- /dev/null
+++ b/src/image_saver/CMakeLists.txt
@@ -0,0 +1,217 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(image_saver)
+
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ cv_bridge
+ roscpp
+ sensor_msgs
+)
+
+
+find_package(yaml-cpp REQUIRED)
+find_package(OpenCV REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# sensor_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+# LIBRARIES image_saver
+# CATKIN_DEPENDS cv_bridge roscpp sensor_msgs
+# DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+include
+ ${catkin_INCLUDE_DIRS}
+ yaml_config
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+# src/${PROJECT_NAME}/image_saver.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+add_executable(${PROJECT_NAME}_node src/main.cc yaml_config/yaml_config.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(${PROJECT_NAME}_node
+ ${catkin_LIBRARIES}
+ ${OpenCV_LIBS}
+ yaml-cpp
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_image_saver.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/src/image_saver/launch/run.launch b/src/image_saver/launch/run.launch
new file mode 100644
index 0000000..3b88f8f
--- /dev/null
+++ b/src/image_saver/launch/run.launch
@@ -0,0 +1,3 @@
+
+
+
diff --git a/src/image_saver/package.xml b/src/image_saver/package.xml
new file mode 100644
index 0000000..b76bb63
--- /dev/null
+++ b/src/image_saver/package.xml
@@ -0,0 +1,68 @@
+
+
+ image_saver
+ 0.0.0
+ The image_saver package
+
+
+
+
+ ubuntu
+
+
+
+
+
+ TODO
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ catkin
+ cv_bridge
+ roscpp
+ sensor_msgs
+ cv_bridge
+ roscpp
+ sensor_msgs
+ cv_bridge
+ roscpp
+ sensor_msgs
+
+
+
+
+
+
+
+
diff --git a/src/image_saver/src/main.cc b/src/image_saver/src/main.cc
new file mode 100644
index 0000000..7f2ba15
--- /dev/null
+++ b/src/image_saver/src/main.cc
@@ -0,0 +1,85 @@
+#include
+#include
+#include
+
+#include
+#include
+#include
+#include
+#include
+
+#include "yaml_config.h"
+
+class ImageSaver {
+ public:
+ ImageSaver(ros::NodeHandle& nh) {
+ std::string subscribe_topic =
+ yaml_config["sub_image_topic"].as();
+ std::cout << "subscribe_topic=" << subscribe_topic << std::endl;
+ // 订阅图像话题
+ image_sub_ =
+ nh.subscribe(subscribe_topic, 1, &ImageSaver::imageCallback, this);
+
+ // 设置保存图像的路径
+ save_path_ = yaml_config["save_path"].as();
+ if (save_path_.empty()) {
+ std::filesystem::path current_dir =
+ std::filesystem::path(__FILE__).parent_path().parent_path();
+ std::filesystem::path yaml_path = current_dir / "images";
+ save_path_ = yaml_path;
+ if (!std::filesystem::exists(yaml_path)) {
+ if (std::filesystem::create_directory(yaml_path)) {
+ std::cout << "Created directory: " << yaml_path << std::endl;
+ } else {
+ std::cerr << "Failed to create directory: " << yaml_path << std::endl;
+ std::terminate();
+ }
+ }
+ }
+ std::cout << "save_path=" << save_path_ << std::endl;
+ frame_count_ = 0;
+
+ save_interval_ = yaml_config["save_interval"].as();
+ std::cout << "save_interval=" << save_interval_ << std::endl;
+ }
+
+ void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
+ // 每3帧保存一张图像
+ if (frame_count_ % save_interval_ == 0) {
+ cv_bridge::CvImagePtr cv_ptr;
+ try {
+ cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
+ } catch (cv_bridge::Exception& e) {
+ ROS_ERROR("cv_bridge exception: %s", e.what());
+ return;
+ }
+
+ // 保存图像
+ std::string file_name =
+ save_path_ + "/" + "image_" + std::to_string(save_count_) + ".jpg";
+ cv::imwrite(file_name, cv_ptr->image);
+ save_count_++;
+ ROS_INFO("Saved image: %s", file_name.c_str());
+ }
+
+ frame_count_++;
+ }
+
+ private:
+ ros::Subscriber image_sub_;
+ std::string save_path_;
+ int frame_count_;
+ int save_count_ = 0;
+ int save_interval_;
+};
+
+int main(int argc, char** argv) {
+ ros::init(argc, argv, "image_saver");
+ ros::NodeHandle nh;
+
+ ImageSaver image_saver(nh);
+
+ ros::spin();
+
+ return 0;
+}
diff --git a/src/image_saver/yaml_config/yaml_config.cpp b/src/image_saver/yaml_config/yaml_config.cpp
new file mode 100644
index 0000000..8cf1661
--- /dev/null
+++ b/src/image_saver/yaml_config/yaml_config.cpp
@@ -0,0 +1,100 @@
+#include "yaml_config.h"
+#include
+#include
+
+YAML::Node ReadConfigYamlFile();
+
+// static std::string yaml_config_path =
+// "/media/Projects/guangpo_lidar/src/guangpo_lidar/yaml_config/yaml_config.yaml";
+static std::string yaml_config_path = []()->std::string{
+ std::filesystem::path current_dir =
+ std::filesystem::path(__FILE__).parent_path();
+ std::filesystem::path yaml_path = current_dir / "yaml_config.yaml";
+ std::cout << "yaml file path: " << yaml_path << std::endl;
+ return yaml_path;
+}();
+
+YAML::Node yaml_config = ReadConfigYamlFile();
+
+YAML::Node ReadConfigYamlFile() {
+ YAML::Node res;
+
+ std::cout << __PRETTY_FUNCTION__ << ": " << std::endl;
+ std::cout << "BEGIN READ FILE: " << yaml_config_path << std::endl;
+ bool read_successful_flag = false;
+ try {
+ // Load the YAML file
+ res = YAML::LoadFile(yaml_config_path);
+ read_successful_flag = true;
+ } catch (const YAML::Exception &e) {
+ std::cerr << "Error while reading the YAML file: " << yaml_config_path
+ << e.what() << std::endl;
+ // gDebugError("[GXT] : Error while reading the YAML file:") << e.what();
+ }
+ if (!read_successful_flag) {
+ // std::cerr << "backtrace:" << __PRETTY_FUNCTION__ << std::endl;
+ // std::cerr << "backtrace:" << __PRETTY_FUNCTION__ << std::endl;
+ // std::cerr << "backtrace:" << __PRETTY_FUNCTION__ << std::endl;
+ std::cerr << "Error while reading the YAML file!" << yaml_config_path
+ << std::endl;
+ std::cerr << "Error while reading the YAML file!" << yaml_config_path
+ << std::endl;
+ std::cerr << "Error while reading the YAML file!" << yaml_config_path
+ << std::endl;
+ std::terminate();
+ }
+ std::cout << "Read yaml config file successfully! " << yaml_config_path
+ << std::endl;
+ return res;
+}
+
+// __attribute((constructor)) inline bool
+// GxtReadConfigYamlFile(const std::string &yaml_config_path) {
+// bool read_successful_flag = false;
+// try {
+// // Load the YAML file
+// YAML::Node config = YAML::LoadFile(yaml_config_path);
+//
+// read_successful_flag = true;
+// // gDebugCol1("READ CONFIG FILE SUCCESSFULLY!");
+//
+// auto quantize = config["quantize"];
+// ADDCONFIG(quantize, quantize_pose_quaternion_bit_width);
+// ADDCONFIG(quantize, quantize_pose_quaternion_il);
+// ADDCONFIG(quantize, quantize_pose_quaternion_bit_width);
+// ADDCONFIG(quantize, quantize_pose_quaternion_il);
+//
+// ADDCONFIG(quantize, quantize_imu_speed_bit_width);
+// ADDCONFIG(quantize, quantize_imu_speed_il);
+// ADDCONFIG(quantize, quantize_imu_accbias_bit_width);
+// ADDCONFIG(quantize, quantize_imu_accbias_il);
+// ADDCONFIG(quantize, quantize_imu_gyrobias_bit_width);
+// ADDCONFIG(quantize, quantize_imu_gyrobias_il);
+//
+// ADDCONFIG(quantize, quantize_inverse_depth_bit_width);
+// ADDCONFIG(quantize, quantize_inverse_depth_il);
+//
+// ADDCONFIG(quantize, quantize_hessian_bit_width);
+// ADDCONFIG(quantize, quantize_hessian_il);
+// ADDCONFIG(quantize, quantize_b_bit_width);
+// ADDCONFIG(quantize, quantize_b_il);
+//
+// auto flag = config["flag"];
+// ADDCONFIG(flag, use_gxt_backend);
+// ADDCONFIG(flag, enable_quantize);
+// } catch (const YAML::Exception &e) {
+// // std::cerr << "Error while reading the YAML file: " << e.what() <<
+// // std::endl;
+// gDebugError("[GXT] : Error while reading the YAML file:") << e.what();
+// }
+//
+// if (!read_successful_flag) {
+// gDebugCol3("\n\n\n==========================================");
+// gDebugCol3("[GXT] : Error while reading the YAML file!");
+// gDebugCol3("[GXT] : Error while reading the YAML file!");
+// gDebugError("[GXT] : Error while reading the YAML file!");
+// std::terminate();
+// }
+//
+// return 0;
+// }
diff --git a/src/image_saver/yaml_config/yaml_config.h b/src/image_saver/yaml_config/yaml_config.h
new file mode 100644
index 0000000..0771ab6
--- /dev/null
+++ b/src/image_saver/yaml_config/yaml_config.h
@@ -0,0 +1,8 @@
+#pragma once
+
+#include
+
+#include
+
+
+extern YAML::Node yaml_config;
diff --git a/src/image_saver/yaml_config/yaml_config.yaml b/src/image_saver/yaml_config/yaml_config.yaml
new file mode 100644
index 0000000..4b9b1c5
--- /dev/null
+++ b/src/image_saver/yaml_config/yaml_config.yaml
@@ -0,0 +1,6 @@
+# 基础ros设置
+sub_image_topic : "/usb_cam/image_raw"
+# 每隔多少图片保存一张
+save_interval : 5
+# 保存图片路径：（为空则默认保存在这个功能包的images目录下）
+save_path : ""
\ No newline at end of file