// 1.包含头文件
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/int32.hpp"
#include "std_msgs/msg/int32_multi_array.hpp"
#include "cansend.h"
#include <iostream>
#include <chrono>
#include <thread>
#include <iostream>
#include <cmath>
#include <stdio.h>
#include <unistd.h>
#include <fcntl.h> // 新增头文件用于设置非阻塞
using namespace std::chrono_literals;
BYTE get_data[2] = {0x01, 0x00};
int encoder_id = 0x0000;
int joint_1data;
int joint_4data;
// 台号1
BYTE OPEN_1_1[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
BYTE OPEN_1_2[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
BYTE OPEN_1_3[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
BYTE OPEN_1_4[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
BYTE CLOSE_1[8] = {0X12, 0X01, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
// 台号2
BYTE OPEN_2_1[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X01};
BYTE OPEN_2_2[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X02, 0X01};
BYTE OPEN_2_3[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X03, 0X01};
BYTE OPEN_2_4[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X04, 0X01};
BYTE CLOSE_2[8] = {0X12, 0X02, 0X00, 0X00, 0X00, 0X00, 0X01, 0X00};
// 多线程
int ret;
int m_run0 = 1;
pthread_t threadid;
// 3.自定义节点类；
class Arm_Encoder : public rclcpp::Node
{
public:
Arm_Encoder(int can_socket) : Node("arm_encoder_node_cpp"), can_socket_(can_socket)
{
// 关键修改1：设置socket为非阻塞模式
int flags = fcntl(can_socket_, F_GETFL, 0);
fcntl(can_socket_, F_SETFL, flags | O_NONBLOCK);
std::cout << "已发送" << std::endl;
SendData(can_socket, encoder_id, false, get_data, 2);
joint1_pub = this->create_publisher<std_msgs::msg::Int32>("joint1_p", 10);
joint4_pub = this->create_publisher<std_msgs::msg::Int32>("joint4_p", 10);
big_valves_sub_ = this->create_subscription<std_msgs::msg::Int32MultiArray>("valve_state", 10,
std::bind(&Arm_Encoder::valveCallback,
this, std::placeholders::_1));
ret = pthread_create(&threadid, NULL, &Arm_Encoder::receive_func, this);
}
~Arm_Encoder()
{
// 关键修改2：添加线程终止机制
pthread_cancel(threadid); // 强制终止线程
pthread_join(threadid, NULL);
}
void ReceiveData()
{
float angle_joint1;
float angle_joint4;
struct can_frame frame;
struct sockaddr_can addr;
socklen_t len = sizeof(addr);
auto joint1_p = std_msgs::msg::Int32();
auto joint4_p = std_msgs::msg::Int32();
while (rclcpp::ok()) // 关键修改3：添加退出条件检查
{
int nbytes = recvfrom(can_socket_, &frame, sizeof(frame), 0,
(struct sockaddr *)&addr, &len);
if (nbytes < 0)
{
if (errno == EAGAIN)
{ // 非阻塞模式无数据
std::this_thread::sleep_for(10ms);
continue;
}
break;
}
// 原有数据处理逻辑保持不变
if (frame.can_id == 0x181)
{
// std::cout << "RX ID:0x" << std::hex << frame.can_id << std::endl;
for (int i = 0; i < frame.can_dlc; i++)
{
uint8_t receiveData[4];
// std::cout << " data:" << std::hex << (int)frame.data[i];
receiveData[i] = (int)frame.data[i];
uint8_t changeData[3];
uint32_t hexNumber = 0;
int32_t encoder2_data;
changeData[0] = receiveData[2];
changeData[1] = receiveData[1];
changeData[2] = receiveData[0];
// printf("\n");
// 将data中的三个字节组合成一个32位的十六进制数
hexNumber |= changeData[0]; // 将第一个字节赋值给hexNumber的最高8位
hexNumber <<= 8; // 左移8位，为下一个字节腾出空间
hexNumber |= changeData[1]; // 将第二个字节赋值给hexNumber的次高8位
hexNumber <<= 8; // 左移8位，为下一个字节腾出空间
hexNumber |= changeData[2]; // 将第三个字节赋值给hexNumber的最低8位
// printf("hexNumber: 0x%06X\n", hexNumber);
// 检查符号位
if (hexNumber & 0x800000)
{
// 如果符号位为1，表示负数
// 将补码转换为正数的补码表示
// uint32_t positiveComplement = ~hexNumber + 1;
// // 将得到的绝对值取负
// int32_t negativeValue = -((int32_t)positiveComplement);
hexNumber |= 0xFF000000; // 扩展符号位到32位
encoder2_data = hexNumber;
// int32_t negativeValue = (int32_t)hexNumber; // 直接转换为有符号
// encoder2_data = negativeValue;
}
else
{
encoder2_data = hexNumber;
// 如果符号位为0，表示正数
}
angle_joint1 = ((float)encoder2_data) * 360 / 16384;
// 需要四舍五入成整数，然后以Int形式发布
// joint_1data = round(angle_joint1 * 1.175);
joint_1data = round(angle_joint1 / (102 * 3.92));
// joint_1data = 5;
// printf("角度是：%d \n", joint_1data);
joint1_p.data = joint_1data;
// joint1_pub->publish(joint1_p);
}
}
else if (frame.can_id == 0x182)
{
// std::cout << "RX ID:0x" << std::hex << frame.can_id << std::endl;
for (int i = 0; i < frame.can_dlc; i++)
{
uint8_t receiveData[4];
// std::cout << " data:" << std::hex << (int)frame.data[i];
receiveData[i] = (int)frame.data[i];
uint8_t changeData4[3];
uint32_t hexNumber4 = 0;
int32_t encoder4_data;
changeData4[0] = receiveData[2];
changeData4[1] = receiveData[1];
changeData4[2] = receiveData[0];
// printf("\n");
// 将data中的三个字节组合成一个32位的十六进制数
hexNumber4 |= changeData4[0]; // 将第一个字节赋值给hexNumber的最高8位
hexNumber4 <<= 8; // 左移8位，为下一个字节腾出空间
hexNumber4 |= changeData4[1]; // 将第二个字节赋值给hexNumber的次高8位
hexNumber4 <<= 8; // 左移8位，为下一个字节腾出空间
hexNumber4 |= changeData4[2]; // 将第三个字节赋值给hexNumber的最低8位
// printf("hexNumber4: 0x%06X\n", hexNumber4);
// 检查符号位
if (hexNumber4 & 0x800000)
{
// 如果符号位为1，表示负数
// 将补码转换为正数的补码表示
hexNumber4 |= 0xFF000000; // 扩展符号位到32位
encoder4_data = hexNumber4;
}
else
{
encoder4_data = hexNumber4;
// 如果符号位为0，表示正数
}
angle_joint4 = ((float)encoder4_data) * 360 / 16384;
// 需要四舍五入成整数，然后以Int形式发布
// 此处需根据实际情况调整
joint_4data = round(angle_joint4 * 1.232);
// printf("角度是：%f \n", angle_joint4);
joint4_p.data = joint_4data;
// joint4_pub->publish(joint4_p);
}
}
printf("1角度是：%d \n", joint_1data);
joint1_pub->publish(joint1_p);
printf("4角度是：%d \n", joint_4data);
joint4_pub->publish(joint4_p);
}
}
private:
int can_socket_; // 关键修改4：将socket存储为成员变量
int state_first = 0;
int state_second = 0;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint1_pub;
rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr joint4_pub;
rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr big_valves_sub_;
static void *receive_func(void *param)
{
Arm_Encoder *encoder = static_cast<Arm_Encoder *>(param);
encoder->ReceiveData();
return nullptr;
}
void valveCallback(const std_msgs::msg::Int32MultiArray::SharedPtr state_msg)
{
// can卡序号 canid 是由扩展帧 数据 数据长度
state_first = state_msg->data[0];
switch (state_first)
{
case 0:
SendData(can_socket_, 0X01, false, CLOSE_1, 8);
break;
case 1:
SendData(can_socket_, 0X01, false, OPEN_1_1, 8);
break;
case 2:
SendData(can_socket_, 0X01, false, OPEN_1_2, 8);
break;
case 3:
SendData(can_socket_, 0X01, false, OPEN_1_3, 8);
break;
case 4:
SendData(can_socket_, 0X01, false, OPEN_1_4, 8);
break;
}
}
};
int main(int argc, char const *argv[])
{
rclcpp::init(argc, argv);
int can1 = init_can("can1");
// 关键修改5：传递socket到节点
rclcpp::spin(std::make_shared<Arm_Encoder>(can1));
rclcpp::shutdown();
close(can1);
return 0;
}
在代码中，can_socket_ 和 can1 的联系是通过构造函数参数传递和类成员变量的存储来实现的。具体来说，can1 是在main函数中初始化的 CAN 总线套接字（socket），然后通过构造函数传递给 Arm_Encoder 类，并存储为类的成员变量 can_socket_。这样，Arm_Encoder 类中的所有成员函数都可以通过 can_socket_ 来访问 CAN 总线套接字。
具体步骤解释
在 main 函数中初始化 can1：
int can1 = init_can("can1");
这里，can1 是一个整数，表示 CAN 总线套接字的文件描述符。它是通过调用 init_can 函数初始化的。
将 can1 传递给 Arm_Encoder 类：
rclcpp::spin(std::make_shared<Arm_Encoder>(can1));
在这里，can1被传递给 Arm_Encoder 类的构造函数。
在 Arm_Encoder 类的构造函数中接收并存储 can1：
Arm_Encoder(int can_socket) : Node("arm_encoder_node_cpp"), can_socket_(can_socket)
{
// ...
}
构造函数接收一个参数 can_socket，并将它存储为类的成员变量 can_socket_。这样，can_socket_ 就保存了can1的值。
can_socket_(can_socket) 是 C++ 中的成员变量初始化列表语法，用于在构造函数中初始化类的成员变量。这是一种在对象创建时直接给成员变量赋值的方式，而不是在构造函数体中通过赋值语句来完成。
Arm_Encoder(int can_socket) : Node("arm_encoder_node_cpp"), can_socket_(can_socket)
{
// ...
}
Arm_Encoder(int can_socket) 是构造函数的声明，表示构造函数接受一个参数 can_socket。
Node("arm_encoder_node_cpp") 是对基类 rclcpp::Node 的构造函数调用，传递字符串 "arm_encoder_node_cpp" 作为节点名称。
can_socket_(can_socket) 是成员变量初始化列表的一部分，表示将构造函数参数can_socket的值赋给类的成员变量can_socket_
假设有一个类 MyClass，它有一个成员变量 int x 和一个构造函数：
class MyClass {
public:
int x;
MyClass(int value) : x(value) {} // 使用初始化列表初始化 x
};
在这个例子中：
MyClass(int value) 是构造函数，接受一个参数 value。
: x(value) 表示将参数 value 的值直接赋给成员变量 x。
如果不用初始化列表，而是直接在构造函数体中赋值：
class MyClass {
public:
int x;
MyClass(int value) {
x = value; // 在构造函数体中赋值
}
};
这两种方式在功能上是等效的
在类的成员函数中使用 can_socket_：
void valveCallback(const std_msgs::msg::Int32MultiArray::SharedPtr state_msg)
{
state_first = state_msg->data[0];
switch (state_first)
{
case 0:
SendData(can_socket_, 0X01, false, CLOSE_1, 8);
break;
case 1:
SendData(can_socket_, 0X01, false, OPEN_1_1, 8);
break;
case 2:
SendData(can_socket_, 0X01, false, OPEN_1_2, 8);
break;
case 3:
SendData(can_socket_, 0X01, false, OPEN_1_3, 8);
break;
case 4:
SendData(can_socket_, 0X01, false, OPEN_1_4, 8);
break;
}
}
在 valveCallback 函数中，can_socket_ 被用来调用 SendData 函数，从而发送 CAN 总线消息。
总结
can_socket_ 和 can1 的联系是通过以下方式建立的：
can1 是在 main 函数中初始化的 CAN 总线套接字。
can1 通过构造函数传递给 Arm_Encoder类，并存储为成员变量can_socket_。
Arm_Encoder 类的成员函数通过 can_socket_ 来访问和操作 CAN 总线。
这种设计模式使得Arm_Encoder类可以独立于main函数，灵活地使用 CAN 总线套接字，而不需要依赖于全局变量。
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
