gxt_kt/guangpo_lidar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

add image_saver package

Browse Source
This commit is contained in:
gxt_kt 2024-08-06 22:33:15 +08:00
parent fceb1b51fe
commit 2e7243e37a
10 changed files with 516 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.gitignore vendored Normal file
View File

@ -0,0 +1,6 @@
.cache
build
devel
cmake-build-*
.catkin_workspace
/src/CMakeLists.txt

21
README.md
View File

@ -0,0 +1,21 @@
# 光珀项目感知算法
## 功能包guangpo_yolo
使用rk3588运行yolov5，订阅图像数据，识别电梯
再根据识别到的人和电梯，得到电梯的拥挤度
具体拥挤度原理：
识别电梯和人后，根据高度进行单目深度估计
根据估计到的深度和相机内参得到相机坐标系下的3d位置
然后电梯和人距离在一定范围内，可以认定有关联
## 功能包image_saver
订阅图像数据，生成图片，方便录制数据包后标定数据
配置文件在对应包路径下的yaml_config/yaml_config.yaml文件中，可以配置：
1. 订阅的话题
2. 间隔多少帧生成一张图片
3. 生成文件的保存路径

2
src/image_saver/.gitignore vendored Normal file
View File

@ -0,0 +1,2 @@
/images
images.*

217
src/image_saver/CMakeLists.txt Normal file
View File

@ -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)

3
src/image_saver/launch/run.launch Normal file
View File

@ -0,0 +1,3 @@
<launch>
<node pkg="image_saver" type="image_saver_node" name="image_saver_node" output="screen" ></node>
</launch>

68
src/image_saver/package.xml Normal file
View File

@ -0,0 +1,68 @@
<?xml version="1.0"?>
<package format="2">
<name>image_saver</name>
<version>0.0.0</version>
<description>The image_saver package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="ubuntu@todo.todo">ubuntu</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>TODO</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/image_saver</url> -->
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_export_depend>cv_bridge</build_export_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>sensor_msgs</build_export_depend>
<exec_depend>cv_bridge</exec_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

85
src/image_saver/src/main.cc Normal file
View File

@ -0,0 +1,85 @@
#include <cv_bridge/cv_bridge.h>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <exception>
#include <filesystem>
#include <iostream>
#include <opencv2/opencv.hpp>
#include <string>
#include "yaml_config.h"
class ImageSaver {
public:
ImageSaver(ros::NodeHandle& nh) {
std::string subscribe_topic =
yaml_config["sub_image_topic"].as<std::string>();
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<std::string>();
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<int>();
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;
}

100
src/image_saver/yaml_config/yaml_config.cpp Normal file
View File

@ -0,0 +1,100 @@
#include "yaml_config.h"
#include <iostream>
#include <filesystem>
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;
// }

8
src/image_saver/yaml_config/yaml_config.h Normal file
View File

@ -0,0 +1,8 @@
#pragma once
#include <yaml-cpp/yaml.h>
#include <string>
extern YAML::Node yaml_config;

6
src/image_saver/yaml_config/yaml_config.yaml Normal file
View File

@ -0,0 +1,6 @@
# 基础ros设置
sub_image_topic : "/usb_cam/image_raw"
# 每隔多少图片保存一张
save_interval : 5
# 保存图片路径：（为空则默认保存在这个功能包的images目录下）
save_path : ""