블루투스 최종 구현

#include <ros/ros.h> #include <std_msgs/String.h> #include <std_msgs/Float32.h> #include <queue> #include <iostream> #include <vector> #include <fstream> #include <string> #include <sstream> #include <thread> #include <boost/multiprecision/cpp_dec_float.hpp> // #include <boost/multiprecision/detail/functions/pow.hpp> #define CAL_DISTANCE(rssi, alpha, n, offset) (std::pow(10.0, ((alpha) - (rssi)) / (10.0 * (n))) + (offset)) //////////////////////////////////// // double rssi; std::queue<double> rssiQueue; // bool initialized = false; std::vector<double> rssi_data; /////////////////////////////////// const int avg_num = 15; class KalmanFilter { public: KalmanFilter(double processNoise = 0.05, double measurementNoise = 20) : processNoise(processNoise), measurementNoise(measurementNoise), predictedRSSI(0), initialized(false),errorCovariance(100) {} inline double filtering(double rssi) { if (!initialized) { initialized = true; predictedRSSI = rssi; } else { double predictedRSSI = this->predictedRSSI; double errorCovariance = this->errorCovariance + processNoise; double kalmanGain = errorCovariance / (errorCovariance + measurementNoise); this->predictedRSSI = predictedRSSI + kalmanGain * (rssi - predictedRSSI); this->errorCovariance = (1 - kalmanGain) * errorCovariance; } return this->predictedRSSI; } private: bool initialized; double processNoise; double measurementNoise; double predictedRSSI; double errorCovariance; }; class MovAvgFilter { public: MovAvgFilter(int _n = avg_num) : n(_n) { for (int i = 0; i < _n; ++i) { xBuf.push(1.0); } prevAvg = 0.0; } double filtering(double x) { double front = xBuf.front(); xBuf.pop(); xBuf.push(x); double avg = prevAvg + (x - front) / n; prevAvg = avg; return avg; } private: double prevAvg; std::queue<double> xBuf; int n; }; class SmoothingFilter { public: SmoothingFilter(double alpha = 0.2) : alpha(alpha), initialized(false),smoothedValue(0) {} double filtering(double value) { if (!initialized) { initialized = true; smoothedValue = value; } else { smoothedValue = alpha * value + (1 - alpha) * smoothedValue; } return smoothedValue; } private: double alpha; bool initialized; double smoothedValue; }; // double cal_distance(double rssi, double alpha = -55.0, double n = 2.0, double offset = 0.0) { // double distance = pow(10.0, (alpha - rssi) / (10.0 * n)); // return distance + offset; // } double cal_distance(double rssi, double alpha = -52.0, double n = 2.0, double offset = 0.0) { using namespace boost::multiprecision; cpp_dec_float_100 distance = pow(cpp_dec_float_100(10), (alpha - rssi) / (10.0 * n)); return static_cast<double>(distance + offset); } KalmanFilter filter; KalmanFilter filter_2; KalmanFilter filter_3; void rssi_callback(const std_msgs::String& data) { if (data.data.empty()) { return; } else if (data.data == "OK+CONNF") { return; } else if(data.data.find("OK+Get:") != std::string::npos) // { // // Create a separate thread for filtering and logging // SmoothingFilter filter; // KalmanFilter filter_2; // std::string line = data.data; // std::string remove_line = line.substr(7); // Remove "OK+Get:" // double rssi = std::stod(remove_line); // std::thread dataProcessingThread([data](){ // SmoothingFilter filter; // KalmanFilter filter_2; // std::string line = data.data; // std::string remove_line = line.substr(7); // Remove "OK+Get:" // double rssi = std::stod(remove_line); // double filtered_rssi = rssi; // // double filtered_rssi = filter.filtering(rssi); // // double filtered_rssi_2 = filter_2.filtering(filtered_rssi); // double final_distance = cal_distance(filtered_rssi); // // double final_distance = CAL_DISTANCE(filtered_rssi,-55.0 ,2.0, 0.0); // // double filtered_distance = filter_2.filtering(final_distance); // // Log the data in a separate thread // ROS_INFO("RSSI to Distance : %f", final_distance); // std::string path = "/home/kym/catkin_ws/src/arduino/src/distance_log.txt"; // std::ofstream logFile(path, std::ios::app); // logFile << filtered_rssi << std::endl; // logFile.close(); // }); // dataProcessingThread.detach(); // } { // Create a separate thread for filtering and logging std::string line = data.data; std::string remove_line = line.substr(7); // Remove "OK+Get:" double rssi = std::stod(remove_line); // rssiQueue.push(rssi); double filtered_rssi = filter.filtering(rssi); // double filtered_rssi_2 = filter_2.filtering(filtered_rssi); // double final_distance = cal_distance(filtered_rssi); double final_distance = CAL_DISTANCE(filtered_rssi,-60.0 ,1.0, 0.0); // double filtered_distance = filter_3.filtering(final_distance); // Log the data in a separate thread ROS_INFO("RSSI to Distance : %f", final_distance); std::string path = "/home/kym/catkin_ws/src/arduino/src/distance_log.txt"; std::ofstream logFile(path, std::ios::app); logFile << final_distance << std::endl; logFile.close(); } } double run() { std::string path = "/home/kym/catkin_ws/src/arduino/src/distance_log.txt"; std::vector<double> original_data; std::vector<double> filtered_data; std::vector<double> distance_list; // 파일에서 원본 데이터 읽어오기 std::ifstream file(path); if (file.is_open()) { double rssi; while (file >> rssi) { original_data.push_back(rssi); } file.close(); } else { std::cerr << "Failed to open file." << std::endl; return 1; } KalmanFilter filter_1; for (double rssi : original_data) { double filtered_rssi = filter_1.filtering(rssi); filtered_data.push_back(filtered_rssi); } for (double rssi : filtered_data) { double distance = cal_distance(rssi); distance_list.push_back(distance); } // 이제 distance_list를 사용하여 거리 데이터를 처리할 수 있습니다. for (double distance : distance_list) { std::cout << "Distance: " << distance << std::endl; } return 0.0; } int main(int argc, char** argv) { ros::init(argc, argv, "rssi_listener"); ros::NodeHandle nh; std::string path_2 = "/home/kym/catkin_ws/src/arduino/src/distance_log.txt"; std::ofstream logFileTemp(path_2); logFileTemp.close(); KalmanFilter filter; try { std::cout << "hi " << std::endl; ros::Subscriber sub = nh.subscribe("/rssi", 10, rssi_callback); // run(); // cal_rssi(); // rssi_pub = nh.advertise<std_msgs::Float32> ("raw_rssi",1); ros::spin(); } catch (ros::Exception& e) { // Handle exceptions } return 0; }

cmake_minimum_required(VERSION 3.0.2) project(arduino) ## 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 roscpp rospy std_msgs message_generation ) find_package(Boost REQUIRED COMPONENTS system) ## 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 std_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 arduino # CATKIN_DEPENDS roscpp rospy std_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} ) ## Declare a C++ library # add_library(arduino src/write_ros_2.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(write_ros_2 src/write_ros_2.cpp) add_dependencies(write_ros_2 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) # target_link_libraries(write_ros_2 ${catkin_LIBRARIES} ${PCL_LIBRARIES}) target_link_libraries(write_ros_2 ${catkin_LIBRARIES} ${Boost_LIBRARIES}) ## 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} # ) ############# ## 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_arduino.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)