From 080af2f7fa79218d4289f88eaa547f17e8a6622b Mon Sep 17 00:00:00 2001 From: NuoDaJia02 Date: Sun, 2 Nov 2025 15:42:32 +0800 Subject: [PATCH] code optimization --- .vscode/c_cpp_properties.json | 18 ++ .vscode/launch.json | 24 ++ .vscode/settings.json | 208 +++++++++++------- drv_test/CMakeLists.txt | 9 +- drv_test/src/main.cpp | 95 +++++++- drv_test/src/xmain.cpp | 187 ++++++++++++++++ .../ethercat_control/src/ethercat_core.cpp | 8 +- img_dev/launch/img_dev.launch.py | 18 +- img_dev/src/img_dev_node.cpp | 4 +- img_dev/src/main.cpp | 5 +- imu_dev/src/main.cpp | 55 +++++ motor_dev/src/main.cpp | 12 +- motor_dev/src/rs485_driver.cpp | 6 + .../src/base_realsense_node.cpp | 2 + .../realsense2_camera/src/ros_sensor.cpp | 1 + .../realsense2_camera/src/rs_node_setup.cpp | 1 + 16 files changed, 549 insertions(+), 104 deletions(-) create mode 100644 .vscode/c_cpp_properties.json create mode 100644 .vscode/launch.json create mode 100644 drv_test/src/xmain.cpp diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..c2098a2 --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,18 @@ +{ + "configurations": [ + { + "name": "linux-gcc-x64", + "includePath": [ + "${workspaceFolder}/**" + ], + "compilerPath": "/usr/bin/gcc", + "cStandard": "${default}", + "cppStandard": "${default}", + "intelliSenseMode": "linux-gcc-x64", + "compilerArgs": [ + "" + ] + } + ], + "version": 4 +} \ No newline at end of file diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 0000000..92ea7b1 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,24 @@ +{ + "version": "0.2.0", + "configurations": [ + { + "name": "C/C++ Runner: Debug Session", + "type": "cppdbg", + "request": "launch", + "args": [], + "stopAtEntry": false, + "externalConsole": false, + "cwd": "/home/demo/hivecore_robot_os1/hivecore_robot_drivers/motor_dev/src", + "program": "/home/demo/hivecore_robot_os1/hivecore_robot_drivers/motor_dev/src/build/Debug/outDebug", + "MIMode": "gdb", + "miDebuggerPath": "gdb", + "setupCommands": [ + { + "description": "Enable pretty-printing for gdb", + "text": "-enable-pretty-printing", + "ignoreFailures": true + } + ] + } + ] +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 11a96ca..42b90a6 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,77 +1,135 @@ { - "files.associations": { - "cctype": "cpp", - "clocale": "cpp", - "cmath": "cpp", - "csignal": "cpp", - "cstdarg": "cpp", - "cstddef": "cpp", - "cstdio": "cpp", - "cstdlib": "cpp", - "cstring": "cpp", - "ctime": "cpp", - "cwchar": "cpp", - "cwctype": "cpp", - "any": "cpp", - "array": "cpp", - "atomic": "cpp", - "bit": "cpp", - "bitset": "cpp", - "chrono": "cpp", - "codecvt": "cpp", - "compare": "cpp", - "complex": "cpp", - "concepts": "cpp", - "condition_variable": "cpp", - "cstdint": "cpp", - "deque": "cpp", - "forward_list": "cpp", - "list": "cpp", - "map": "cpp", - "set": "cpp", - "string": "cpp", - "unordered_map": "cpp", - "unordered_set": "cpp", - "vector": "cpp", - "exception": "cpp", - "algorithm": "cpp", - "functional": "cpp", - "iterator": "cpp", - "memory": "cpp", - "memory_resource": "cpp", - "numeric": "cpp", - "optional": "cpp", - "random": "cpp", - "ratio": "cpp", - "regex": "cpp", - "string_view": "cpp", - "system_error": "cpp", - "tuple": "cpp", - "type_traits": "cpp", - "utility": "cpp", - "fstream": "cpp", - "future": "cpp", - "initializer_list": "cpp", - "iomanip": "cpp", - "iosfwd": "cpp", - "iostream": "cpp", - "istream": "cpp", - "limits": "cpp", - "mutex": "cpp", - "new": "cpp", - "numbers": "cpp", - "ostream": "cpp", - "semaphore": "cpp", - "sstream": "cpp", - "stdexcept": "cpp", - "stop_token": "cpp", - "streambuf": "cpp", - "thread": "cpp", - "cfenv": "cpp", - "cinttypes": "cpp", - "typeindex": "cpp", - "typeinfo": "cpp", - "valarray": "cpp", - "variant": "cpp" - } + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "csignal": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "any": "cpp", + "array": "cpp", + "atomic": "cpp", + "bit": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "codecvt": "cpp", + "compare": "cpp", + "complex": "cpp", + "concepts": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "forward_list": "cpp", + "list": "cpp", + "map": "cpp", + "set": "cpp", + "string": "cpp", + "unordered_map": "cpp", + "unordered_set": "cpp", + "vector": "cpp", + "exception": "cpp", + "algorithm": "cpp", + "functional": "cpp", + "iterator": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "numeric": "cpp", + "optional": "cpp", + "random": "cpp", + "ratio": "cpp", + "regex": "cpp", + "string_view": "cpp", + "system_error": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "fstream": "cpp", + "future": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "numbers": "cpp", + "ostream": "cpp", + "semaphore": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "stop_token": "cpp", + "streambuf": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "valarray": "cpp", + "variant": "cpp" + }, + "C_Cpp_Runner.cCompilerPath": "gcc", + "C_Cpp_Runner.cppCompilerPath": "g++", + "C_Cpp_Runner.debuggerPath": "gdb", + "C_Cpp_Runner.cStandard": "", + "C_Cpp_Runner.cppStandard": "", + "C_Cpp_Runner.msvcBatchPath": "", + "C_Cpp_Runner.useMsvc": false, + "C_Cpp_Runner.warnings": [ + "-Wall", + "-Wextra", + "-Wpedantic", + "-Wshadow", + "-Wformat=2", + "-Wcast-align", + "-Wconversion", + "-Wsign-conversion", + "-Wnull-dereference" + ], + "C_Cpp_Runner.msvcWarnings": [ + "/W4", + "/permissive-", + "/w14242", + "/w14287", + "/w14296", + "/w14311", + "/w14826", + "/w44062", + "/w44242", + "/w14905", + "/w14906", + "/w14263", + "/w44265", + "/w14928" + ], + "C_Cpp_Runner.enableWarnings": true, + "C_Cpp_Runner.warningsAsError": false, + "C_Cpp_Runner.compilerArgs": [], + "C_Cpp_Runner.linkerArgs": [], + "C_Cpp_Runner.includePaths": [], + "C_Cpp_Runner.includeSearch": [ + "*", + "**/*" + ], + "C_Cpp_Runner.excludeSearch": [ + "**/build", + "**/build/**", + "**/.*", + "**/.*/**", + "**/.vscode", + "**/.vscode/**" + ], + "C_Cpp_Runner.useAddressSanitizer": false, + "C_Cpp_Runner.useUndefinedSanitizer": false, + "C_Cpp_Runner.useLeakSanitizer": false, + "C_Cpp_Runner.showCompilationTime": false, + "C_Cpp_Runner.useLinkTimeOptimization": false, + "C_Cpp_Runner.msvcSecureNoWarnings": false, + "Codegeex.RepoIndex": true } \ No newline at end of file diff --git a/drv_test/CMakeLists.txt b/drv_test/CMakeLists.txt index 359f3e9..ffa3a05 100644 --- a/drv_test/CMakeLists.txt +++ b/drv_test/CMakeLists.txt @@ -13,9 +13,12 @@ find_package(motor_dev REQUIRED) find_package(interfaces REQUIRED) find_package(std_msgs REQUIRED) -add_executable(motor_test src/main.cpp) -ament_target_dependencies(motor_test rclcpp motor_dev interfaces) -install(TARGETS motor_test DESTINATION lib/${PROJECT_NAME}) +add_executable(main src/main.cpp) +add_executable(xmain src/xmain.cpp) +ament_target_dependencies(main rclcpp motor_dev interfaces sensor_msgs) +ament_target_dependencies(xmain rclcpp motor_dev interfaces sensor_msgs) +install(TARGETS main DESTINATION lib/${PROJECT_NAME}) +install(TARGETS xmain DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/drv_test/src/main.cpp b/drv_test/src/main.cpp index e9f45ef..006f831 100644 --- a/drv_test/src/main.cpp +++ b/drv_test/src/main.cpp @@ -2,12 +2,37 @@ #include #include "rclcpp/rclcpp.hpp" #include "interfaces/msg/motor_cmd.hpp" +#include "interfaces/srv/motor_info.hpp" +#include "interfaces/srv/motor_param.hpp" #include "std_msgs/msg/string.hpp" - +#include "sensor_msgs/msg/image.hpp" using namespace std::chrono_literals; -using MotorCmd = interfaces::msg::MotorCmd; +static auto g_program_start_time = std::chrono::steady_clock::now(); +double log_printf(const char* format, ...) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast>(now - g_program_start_time); + double seconds = elapsed.count(); + double minute=seconds/60; + char buffer[1024]; + va_list args; + va_start(args, format); + int n = vsnprintf(buffer, sizeof(buffer), format, args); + va_end(args); + + if (n < 0) { + std::fprintf(stderr, "[%.3f] (log error)\n", seconds); + return 0; + } + + std::fprintf(stdout, "[%.3f/%.1f] %s", seconds,minute, buffer); + std::fflush(stdout); + return seconds; +} +using MotorCmd = interfaces::msg::MotorCmd; +using MotorInfo=interfaces::srv::MotorInfo; +using MotorParam=interfaces::srv::MotorParam; class MotorCmdPublisher : public rclcpp::Node { public: @@ -16,17 +41,75 @@ public: { publisher_ = this->create_publisher("/motor_cmd", 10); //publisher_ = this->create_publisher("/led_cmd", 10); - - timer_ = this->create_wall_timer(5000ms, std::bind(&MotorCmdPublisher::timer_callback, this)); + cam_sub=this->create_subscription("/camera/camera/color/image_raw",10,[=](sensor_msgs::msg::Image::SharedPtr img){ + static double last_stamp; + double cur_stamp= log_printf("sub img:%dx%d ",img->width,img->height); + double diff=cur_stamp-last_stamp; + last_stamp=cur_stamp; + log_printf("hz:%.1f\n",1.0/diff); + + }); + //client = create_client("/motor_info"); + ///client = create_client("/motor_param"); + ////timer_ = this->create_wall_timer(5000ms, std::bind(&MotorCmdPublisher::timer_callback, this)); } - + private: int angle1=9999; int angle2=6666; int xcnt=0; + //rclcpp::Client::SharedPtr client; + rclcpp::Client::SharedPtr client; + rclcpp::Subscription::SharedPtr cam_sub; void timer_callback() { -#if 1 + + +auto request = std::make_shared(); +request->motor_id=1; +request->max_speed=20; +request->add_acc=4; +request->dec_acc=5; +/* +auto future = client->async_send_request(request); + +if (future.wait_for(5s) != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "服务调用超时"); + return; +} +auto response=future.get(); +printf("ret :%d\n",response->ret); +*/ + +using ServiceResponseFuture = rclcpp::Client::SharedFuture; + auto response_received_callback = [this](ServiceResponseFuture future) { + auto response = future.get(); + RCLCPP_INFO(this->get_logger(), "Service response: ret = %d", response->ret); + }; + + auto future = client->async_send_request(request, response_received_callback); + + + + +#if 0 +auto request = std::make_shared(); +request->type="bm"; +request->motor_ids.clear(); +request->motor_ids.push_back(static_cast(1)); +request->motor_ids.push_back(static_cast(2)); + +auto future = client->async_send_request(request); + +if (future.wait_for(10s) != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "服务调用超时"); + return; +} + +auto response = future.get(); +printf("ret angle:%.1f,%.1f\n",response->motor_angles[0],response->motor_angles[1]); +#endif +#if 0 auto message = MotorCmd(); message.target = "rs485"; diff --git a/drv_test/src/xmain.cpp b/drv_test/src/xmain.cpp new file mode 100644 index 0000000..85fa9b6 --- /dev/null +++ b/drv_test/src/xmain.cpp @@ -0,0 +1,187 @@ +#include +#include +#include "rclcpp/rclcpp.hpp" +#include "interfaces/msg/motor_cmd.hpp" +#include "interfaces/srv/motor_info.hpp" +#include "interfaces/srv/motor_param.hpp" +#include "std_msgs/msg/string.hpp" +#include "sensor_msgs/msg/image.hpp" +using namespace std::chrono_literals; + +static auto g_program_start_time = std::chrono::steady_clock::now(); + +double log_printf(const char* format, ...) { + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast>(now - g_program_start_time); + double seconds = elapsed.count(); + double minute=seconds/60; + char buffer[1024]; + va_list args; + va_start(args, format); + int n = vsnprintf(buffer, sizeof(buffer), format, args); + va_end(args); + + if (n < 0) { + std::fprintf(stderr, "[%.3f] (log error)\n", seconds); + return 0; + } + + std::fprintf(stdout, "[%.3f/%.1f] %s", seconds,minute, buffer); + std::fflush(stdout); + return seconds; +} +using MotorCmd = interfaces::msg::MotorCmd; +using MotorInfo=interfaces::srv::MotorInfo; +using MotorParam=interfaces::srv::MotorParam; +class MotorCmdPublisher : public rclcpp::Node +{ +public: + MotorCmdPublisher() + : Node("motor_test_node") + { + publisher_ = this->create_publisher("/motor_cmd", 10); + img_pub = this->create_publisher("/color/image_raw",10); + //publisher_ = this->create_publisher("/led_cmd", 10); + cam_sub=this->create_subscription("/camera/color/image_raw",10,[=](sensor_msgs::msg::Image::SharedPtr img){ + static double last_stamp; + double cur_stamp= log_printf("sub img:%dx%d ",img->width,img->height); + double diff=cur_stamp-last_stamp; + last_stamp=cur_stamp; + log_printf("hz:%.1f\n",1.0/diff); + + }); + //client = create_client("/motor_info"); + ///client = create_client("/motor_param"); + timer_ = this->create_wall_timer(33ms, std::bind(&MotorCmdPublisher::timer_callback, this)); + } + +private: + int angle1=9999; + int angle2=6666; + int xcnt=0; + //rclcpp::Client::SharedPtr client; + rclcpp::Client::SharedPtr client; + rclcpp::Subscription::SharedPtr cam_sub; + rclcpp::Publisher::SharedPtr img_pub; + + void timer_callback() + { + auto msg = std::make_unique(); + msg->header.stamp = this->now(); + msg->header.frame_id = "camera_frame"; + msg->height = 720; + msg->width = 1280; + msg->encoding = "rgb8"; // 3通道RGB + msg->is_bigendian = false; + msg->step = msg->width * 3; // 每行字节数 = 宽度 × 通道数 + + // 2. 填充随机数据 (1280x720x3 = 2,764,800字节) + msg->data.resize(msg->height * msg->step); + for (auto& pixel : msg->data) { + pixel = 23;//dist_(rng_); + } + + // 3. 发布消息 + img_pub->publish(std::move(msg)); + static double last_stamp; + double cur_stamp=log_printf("pub msg:"); + double diff=cur_stamp-last_stamp; + log_printf("hz:%.1f\n",1/diff); + last_stamp=cur_stamp; + return; + + +// auto request = std::make_shared(); +// request->motor_id=1; +// request->max_speed=20; +// request->add_acc=4; +// request->dec_acc=5; +/* +auto future = client->async_send_request(request); + +if (future.wait_for(5s) != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "服务调用超时"); + return; +} +auto response=future.get(); +printf("ret :%d\n",response->ret); +*/ + +// using ServiceResponseFuture = rclcpp::Client::SharedFuture; +// auto response_received_callback = [this](ServiceResponseFuture future) { +// auto response = future.get(); +// RCLCPP_INFO(this->get_logger(), "Service response: ret = %d", response->ret); +// }; + +// auto future = client->async_send_request(request, response_received_callback); + + + + +#if 0 +auto request = std::make_shared(); +request->type="bm"; +request->motor_ids.clear(); +request->motor_ids.push_back(static_cast(1)); +request->motor_ids.push_back(static_cast(2)); + +auto future = client->async_send_request(request); + +if (future.wait_for(10s) != std::future_status::ready) { + RCLCPP_ERROR(this->get_logger(), "服务调用超时"); + return; +} + +auto response = future.get(); +printf("ret angle:%.1f,%.1f\n",response->motor_angles[0],response->motor_angles[1]); +#endif +#if 0 + auto message = MotorCmd(); + + message.target = "rs485"; + message.type = "bm"; + message.position = ""; + + message.motor_id.push_back(1); + message.motor_id.push_back(2); + idx+=1; + //angle1=angle1+20; + //angle2=angle2-20; + /* + if(idx%2==0){ + }else{ + angle1=-300; + angle2=300; + } + */ + message.motor_angle.push_back((float)angle1); + message.motor_angle.push_back((float)angle2); + publisher_->publish(message); + RCLCPP_INFO(this->get_logger(), "Published MotorCmd message:%d,%d",angle1,angle2); +#endif +#if 0 + auto message=std_msgs::msg::String(); + xcnt+=1; + if(xcnt%2==0) + message.data="green,10"; + else + message.data="blue,10"; + publisher_->publish(message); + + RCLCPP_INFO(this->get_logger(), "Published LedCmd message:%s",message.data.c_str()); +#endif + //timer_->cancel(); + } + int idx=0; + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + //rclcpp::Publisher::SharedPtr publisher_; +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp b/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp index fe80c3f..fe81481 100644 --- a/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp +++ b/ethercat_dev/src/ethercat_control/src/ethercat_core.cpp @@ -180,6 +180,8 @@ void check_slave_config_states(void) */ int lastStatus = 0; +int motorCount1 = 0; +int motorCount2 = 0; void cyclic_task() { @@ -360,7 +362,8 @@ void cyclic_task() EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2); g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed); - std::cout << " Motor : " << i << " Switched on" << std::endl; + motorCount1 ++; + std::cout << " Motor : " << i << " times : " << motorCount1 << " Switched on" << std::endl; } else if ((status[i] & command[i]) == 0x0023) { EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f); // Operation enabled command[i] = 0x006F; @@ -519,7 +522,8 @@ void cyclic_task() EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv2); g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed); - std::cout << " Motor : " << i << " Switched on" << std::endl; + motorCount2 ++; + std::cout << " Motor : " << i << " times : " << motorCount2 << " Switched on" << std::endl; } else if ((status[i] & command[i]) == 0x0023) { EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x000f); // Operation enabled command[i] = 0x006F; diff --git a/img_dev/launch/img_dev.launch.py b/img_dev/launch/img_dev.launch.py index 751ff99..88d9d37 100644 --- a/img_dev/launch/img_dev.launch.py +++ b/img_dev/launch/img_dev.launch.py @@ -8,8 +8,8 @@ def generate_launch_description(): return LaunchDescription([ # 启动orbbec_camera #ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'multi_camera.launch.py'],output='screen'), - ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'gemini_330_series.launch.py'],output='screen'), - ExecuteProcess(cmd=['sleep', '2'],output='screen'), + # ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'gemini_330_series.launch.py'],output='screen'), + # ExecuteProcess(cmd=['sleep', '2'],output='screen'), ExecuteProcess(cmd=['ros2','launch', 'realsense2_camera','rs_launch.py'],output='screen'), ExecuteProcess(cmd=['sleep', '2'],output='screen'), @@ -17,12 +17,12 @@ def generate_launch_description(): # 等待一段时间确保相机启动完成 # 启动img_dev_node - Node( - package='img_dev', - executable='img_dev_node', - name='img_dev_node', - output='screen', - parameters=[] - ) + # Node( + # package='img_dev', + # executable='img_dev_node', + # name='img_dev_node', + # output='screen', + # parameters=[] + # ) ]) diff --git a/img_dev/src/img_dev_node.cpp b/img_dev/src/img_dev_node.cpp index fa4b932..d375e11 100644 --- a/img_dev/src/img_dev_node.cpp +++ b/img_dev/src/img_dev_node.cpp @@ -21,9 +21,9 @@ void ImageSubscriber::pub_msg(const sensor_msgs::msg::Image& c_img,const sensor_ img_msg->karr=cfg.k_arr; img_msg->darr=cfg.d_arr; img_pub->publish(*img_msg); - pub_cnt+=1; + // pub_cnt+=1; - std::cout<<"pub msg["<image_color.width<<"x"<image_color.height<<";"<darr[0]<<","<darr[1]<<",k:"<karr[0]<<","<karr[1]<image_color.width<<"x"<image_color.height<<";"<darr[0]<<","<darr[1]<<",k:"<karr[0]<<","<karr[1]<pub_msg(c_img,d_img,cfg1); } int main(int argc, char* argv[]) { + setenv("ROS_DISABLE_SHM", "1", 1); //禁用共享内存 rclcpp::init(argc, argv); cur_node=make_shared(); if(!cur_node){ RCLCPP_ERROR(rclcpp::get_logger("main"),"img_dev node create error!!!!"); return -1; } + + #if 0 //////////////////奥比中光1 START/////////////////////// rclcpp::Subscription::SharedPtr camera01_info=cur_node->create_subscription("/camera/color/camera_info", 10, [&](const sensor_msgs::msg::CameraInfo& info){ @@ -53,7 +56,7 @@ int main(int argc, char* argv[]) { auto sync0 = make_shared>(SyncPolicy(10), c_sub0, d_sub0); sync0->registerCallback(&sync_cb0); //////////////////奥比中光1 END/////////////////////// - +#endif #if 0 //////////////////奥比中光2 START/////////////////////// diff --git a/imu_dev/src/main.cpp b/imu_dev/src/main.cpp index d825651..fa24e38 100644 --- a/imu_dev/src/main.cpp +++ b/imu_dev/src/main.cpp @@ -1,7 +1,48 @@ #include "rclcpp/rclcpp.hpp" #include "interfaces/msg/imu_msg.hpp" #include "sensor_msgs/msg/imu.hpp" +#include using namespace std; + +void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw) +{ + double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz); + if (norm < 1e-12) { // 避免除以零 + roll = 0; + pitch = 0; + yaw = 0; + return; + } + + qw /= norm; + qx /= norm; + qy /= norm; + qz /= norm; + + // 计算Roll角（X轴） + roll = atan2(2 * (qw*qx + qy*qz), 1 - 2 * (qx*qx + qy*qy)); + + // 计算Pitch角（Y轴），限制输入范围避免数值误差（asin输入需在[-1,1]） + double sin_pitch = 2 * (qw*qy - qz*qx); + sin_pitch = std::max(std::min(sin_pitch, 1.0), -1.0); + pitch = asin(sin_pitch); + + // 计算Yaw角（Z轴） + yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz)); +} + +// std::ofstream rpy_file("rpy.txt"); +void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw) +{ + QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw); + + roll = roll * 180.0 / 3.1415926; + pitch = pitch * 180.0 / 3.1415926; + yaw = yaw * 180.0 / 3.1415926; + // rpy_file<publish(imu_msg); }); ///livox/imu @@ -35,4 +89,5 @@ int main(int argc, char **argv) rclcpp::spin(node); rclcpp::shutdown(); + rpy_file.close(); } diff --git a/motor_dev/src/main.cpp b/motor_dev/src/main.cpp index 575de29..74dd2a0 100644 --- a/motor_dev/src/main.cpp +++ b/motor_dev/src/main.cpp @@ -49,13 +49,13 @@ int main(int argc,char* argv[]){ rs485_driver_.bm_set_mode(2,4); rs485_driver_.bm_set_enable(2,2); - rs485_driver_.bm_set_param(1,77,15); - rs485_driver_.bm_set_param(1,78,4); - rs485_driver_.bm_set_param(1,79,4); + rs485_driver_.bm_set_param(1,77,40); + rs485_driver_.bm_set_param(1,78,10); + rs485_driver_.bm_set_param(1,79,10); //rs485_driver_.bm_set_param(1,28,4); - rs485_driver_.bm_set_param(2,77,15); - rs485_driver_.bm_set_param(2,78,4); - rs485_driver_.bm_set_param(2,79,4); + rs485_driver_.bm_set_param(2,77,40); + rs485_driver_.bm_set_param(2,78,10); + rs485_driver_.bm_set_param(2,79,10); //rs485_driver_.mt_set_pos(1,-5); //init /////rs485_driver_.bm_set_mode(1,3);//位置模式 diff --git a/motor_dev/src/rs485_driver.cpp b/motor_dev/src/rs485_driver.cpp index 09a9c1f..78ddd6e 100644 --- a/motor_dev/src/rs485_driver.cpp +++ b/motor_dev/src/rs485_driver.cpp @@ -174,10 +174,12 @@ namespace motor_dev { static int idx=0; idx+=1; +#ifdef hehe printf("[%03d]-->",idx); for(auto d:data) printf("%02x ",d); printf("\n"); +#endif ssize_t bytes_written = write(com_fd_, data.data(), data.size()); if (bytes_written != static_cast(data.size())) { @@ -420,10 +422,12 @@ namespace motor_dev data.clear(); ret=receiveData(data, 20, 5); if(ret){ +#ifdef hehe printf("<---:"); for(auto d:data) printf("%02x ",d); printf("\n"); +#endif } } return 0; @@ -448,10 +452,12 @@ namespace motor_dev data.clear(); ret=receiveData(data, 11, 1); if(ret){ +#ifdef hehe printf("<---:"); for(auto d:data) printf("%02x ",d); printf("\n"); +#endif uint8_t nid=0x70+motor_id; if(data[0]==0&&data[1]==nid){ //uint16_t pos = (data[2]<<8)+(data[3]&0xff); diff --git a/realsense/realsense2_camera/src/base_realsense_node.cpp b/realsense/realsense2_camera/src/base_realsense_node.cpp index 84a47ba..19f9317 100755 --- a/realsense/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense/realsense2_camera/src/base_realsense_node.cpp @@ -562,6 +562,7 @@ void BaseRealSenseNode::imu_callback(rs2::frame frame) void BaseRealSenseNode::frame_callback(rs2::frame frame) { + ///ROS_DEBUG("hehe recv realsense frame!!!! "); if (_synced_imu_publisher) _synced_imu_publisher->Pause(); double frame_time = frame.get_timestamp(); @@ -689,6 +690,7 @@ void BaseRealSenseNode::frame_callback(rs2::frame frame) void BaseRealSenseNode::multiple_message_callback(rs2::frame frame, imu_sync_method sync_method) { + ///ROS_DEBUG("hehe multiple_message_callback"); auto stream = frame.get_profile().stream_type(); switch (stream) { diff --git a/realsense/realsense2_camera/src/ros_sensor.cpp b/realsense/realsense2_camera/src/ros_sensor.cpp index 3cfa59e..69d5099 100644 --- a/realsense/realsense2_camera/src/ros_sensor.cpp +++ b/realsense/realsense2_camera/src/ros_sensor.cpp @@ -63,6 +63,7 @@ RosSensor::RosSensor(rs2::sensor sensor, stream_index_pair sip{stream_type, stream_index}; try { + ////ROS_DEBUG("hehe RosSensor"); _origin_frame_callback(frame); if (_frequency_diagnostics.find(sip) != _frequency_diagnostics.end()) _frequency_diagnostics.at(sip).Tick(); diff --git a/realsense/realsense2_camera/src/rs_node_setup.cpp b/realsense/realsense2_camera/src/rs_node_setup.cpp index 66987ba..d5e776b 100755 --- a/realsense/realsense2_camera/src/rs_node_setup.cpp +++ b/realsense/realsense2_camera/src/rs_node_setup.cpp @@ -181,6 +181,7 @@ void BaseRealSenseNode::setAvailableSensors() void BaseRealSenseNode::setCallbackFunctions() { + //ROS_DEBUG("hehe setCallbackFunctions"); _asyncer.start([this](rs2::frame f) { frame_callback(f);