HiveCoreRD/hivecore_robot_drivers
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

code optimization

Browse Source
This commit is contained in:
NuoDaJia02
2025-11-02 15:42:32 +08:00
parent 986004aaa4
commit 080af2f7fa
16 changed files with 549 additions and 104 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
.vscode/c_cpp_properties.json vendored Normal file
View File

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

24
.vscode/launch.json vendored Normal file
View File

@@ -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
}
]
}
]
}

208
.vscode/settings.json vendored
View File

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

9
drv_test/CMakeLists.txt
View File

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

95
drv_test/src/main.cpp
View File

@@ -2,12 +2,37 @@
#include <memory>
#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<std::chrono::duration<double>>(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<MotorCmd>("/motor_cmd", 10);
//publisher_ = this->create_publisher<std_msgs::msg::String>("/led_cmd", 10);
timer_ = this->create_wall_timer(5000ms, std::bind(&MotorCmdPublisher::timer_callback, this));
cam_sub=this->create_subscription<sensor_msgs::msg::Image>("/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<MotorInfo>("/motor_info");
///client = create_client<MotorParam>("/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<interfaces::srv::MotorInfo>::SharedPtr client;
rclcpp::Client<interfaces::srv::MotorParam>::SharedPtr client;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam_sub;
void timer_callback()
{
#if 1
auto request = std::make_shared<MotorParam::Request>();
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<interfaces::srv::MotorParam>::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<MotorInfo::Request>();
request->type="bm";
request->motor_ids.clear();
request->motor_ids.push_back(static_cast<uint8_t>(1));
request->motor_ids.push_back(static_cast<uint8_t>(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";

187
drv_test/src/xmain.cpp Normal file
View File

@@ -0,0 +1,187 @@
#include <chrono>
#include <memory>
#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<std::chrono::duration<double>>(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<MotorCmd>("/motor_cmd", 10);
img_pub = this->create_publisher<sensor_msgs::msg::Image>("/color/image_raw",10);
//publisher_ = this->create_publisher<std_msgs::msg::String>("/led_cmd", 10);
cam_sub=this->create_subscription<sensor_msgs::msg::Image>("/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<MotorInfo>("/motor_info");
///client = create_client<MotorParam>("/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<interfaces::srv::MotorInfo>::SharedPtr client;
rclcpp::Client<interfaces::srv::MotorParam>::SharedPtr client;
rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam_sub;
rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr img_pub;
void timer_callback()
{
auto msg = std::make_unique<sensor_msgs::msg::Image>();
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<MotorParam::Request>();
// 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<interfaces::srv::MotorParam>::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<MotorInfo::Request>();
request->type="bm";
request->motor_ids.clear();
request->motor_ids.push_back(static_cast<uint8_t>(1));
request->motor_ids.push_back(static_cast<uint8_t>(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<MotorCmd>::SharedPtr publisher_;
//rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
};
int main(int argc, char * argv[])
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<MotorCmdPublisher>());
rclcpp::shutdown();
return 0;
}

8
ethercat_dev/src/ethercat_control/src/ethercat_core.cpp
View File

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

18
img_dev/launch/img_dev.launch.py
View File

@@ -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=[]
# )
])

4
img_dev/src/img_dev_node.cpp
View File

@@ -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["<<pub_cnt<<"]:"<<img_msg->image_color.width<<"x"<<img_msg->image_color.height<<";"<<img_msg->darr[0]<<","<<img_msg->darr[1]<<",k:"<<img_msg->karr[0]<<","<<img_msg->karr[1]<<std::endl;
// std::cout<<"pub msg["<<pub_cnt<<"]:"<<img_msg->image_color.width<<"x"<<img_msg->image_color.height<<";"<<img_msg->darr[0]<<","<<img_msg->darr[1]<<",k:"<<img_msg->karr[0]<<","<<img_msg->karr[1]<<std::endl;
}
}

5
img_dev/src/main.cpp
View File

@@ -27,12 +27,15 @@ void sync_cb1(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Imag
cur_node->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<ImageSubscriber>();
if(!cur_node){
RCLCPP_ERROR(rclcpp::get_logger("main"),"img_dev node create error!!!!");
return -1;
}
#if 0
//////////////////奥比中光1 START///////////////////////
rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera01_info=cur_node->create_subscription<sensor_msgs::msg::CameraInfo>("/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<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), c_sub0, d_sub0);
sync0->registerCallback(&sync_cb0);
//////////////////奥比中光1 END///////////////////////
#endif
#if 0
//////////////////奥比中光2 START///////////////////////

55
imu_dev/src/main.cpp
View File

@@ -1,7 +1,48 @@
#include "rclcpp/rclcpp.hpp"
#include "interfaces/msg/imu_msg.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include <fstream>
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<<roll<<","<<pitch<<","<<yaw<<std::endl;
std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
}
int main(int argc, char **argv)
{
rclcpp::init(argc, argv);
@@ -16,6 +57,19 @@ int main(int argc, char **argv)
imu_msg.position="front";
imu_msg.type="None";
imu_msg.imu=*msg;
double gyroValues_[3];
QuaternionToRPYDeg(
imu_msg.imu.orientation.w,
imu_msg.imu.orientation.x,
imu_msg.imu.orientation.y,
imu_msg.imu.orientation.z,
gyroValues_[0],
gyroValues_[1],
gyroValues_[2]
);
imu_pub->publish(imu_msg);
});
///livox/imu
@@ -35,4 +89,5 @@ int main(int argc, char **argv)
rclcpp::spin(node);
rclcpp::shutdown();
rpy_file.close();
}

12
motor_dev/src/main.cpp
View File

@@ -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);//位置模式

6
motor_dev/src/rs485_driver.cpp
View File

@@ -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<ssize_t>(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);

2
realsense/realsense2_camera/src/base_realsense_node.cpp
View File

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

1
realsense/realsense2_camera/src/ros_sensor.cpp
View File

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

1
realsense/realsense2_camera/src/rs_node_setup.cpp
View File

@@ -181,6 +181,7 @@ void BaseRealSenseNode::setAvailableSensors()
void BaseRealSenseNode::setCallbackFunctions()
{
//ROS_DEBUG("hehe setCallbackFunctions");
_asyncer.start([this](rs2::frame f)
{
frame_callback(f);