ros下使用CANopen与电机进行通讯

最新推荐文章于 2025-02-23 20:07:26 发布

旅行......恶魔...

最新推荐文章于 2025-02-23 20:07:26 发布

阅读量3.7k

点赞数 9

分类专栏：创建自己的里程计文章标签：自动驾驶 c++ 机器人

本文链接：https://blog.csdn.net/weixin_57445413/article/details/130527541

版权

创建自己的里程计专栏收录该内容

3 篇文章

订阅专栏

CAN总线的通讯模式包括PDO和SDO，PDO用于实时数据传输，类似ROS的话题，而SDO用于配置参数，类似ROS的服务。文章介绍了如何初始化CAN接口以及在CANopen协议下进行通信。当设备波特率不一致或接线错误时，通信可能会失败。同时，文章给出了ROS下CAN消息与话题消息转换的例子，以及如何通过ROS节点控制CAN设备的运动和工作模式。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

CAN总线的通讯模式：

PDO：过程数据对象（Process Data Object），过程数据的发送，实时、速度快，提供对设备应用对象的直接访问通道，它用来传输实时短帧数据，具有较高的优先权。PDO 传输的数据必须少于或等于 8 个字节，在应用层上不包含传输控制信息，报文利用率极高通常编码器的数据读取以及车轮的控制通过PDO。

SDO：服务数据对象（Service Data Obiect），服务数据的发送接收，实时性要求不高，SDO一般用来配置和获得节点的配置参数例如修改、修改字典\od对象、电机的工作模式或读取电机的工作状态。

与ROS的通讯方式比较：

CAN总线的通讯ROS的通信非常相似，PDO模式与ros的话题消息通信机制高度相似，而SDO与ros的服务高度相似。但can总线的通信是通过broadcast的形式，ros通信中采用的分布式（订阅

）的形式。

在设备支持CANopen的情况下需要通过运行以下指令对CAN口进行初始化：

sudo modprobe can
sudo ip link set can1 type can bitrate 500000
sudo ip link set can1 up

通常以下两种情况会导致两台设备无法通过can通信：

1两台设备的can波特率不一致

2接线错误

随后运行ros-canopen包将can消息转化为话题消息（默认安装连接的是can0如果想用其他can口需要修改源码然后重新编译)

sdo模式下指令为：

站号ID # 操作符(1B) 索引(2B) 子索引(1B) 数据(4B) 例如

cansend can1 601#2F606000FDFF

pdo模式下指令为：

站号ID # 数据(4B) 例如

cansend can1 201#00400600

#include <ros/ros.h>
#include <ros/spinner.h>
#include <std_msgs/String.h>
#include <tf/transform_broadcaster.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Twist.h>
#include <std_msgs/String.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Float32.h>
#include <std_msgs/Float32MultiArray.h>
#include <socketcan_bridge/topic_to_socketcan.h>
#include <socketcan_bridge/socketcan_to_topic.h>
#include <can_msgs/Frame.h>
#include <socketcan_interface/socketcan.h>

#include <iostream>

#define LEFT_MOTOR_CAN_ID 0x601
#define RIGHT_MOTOR_CAN_ID 0x602

#define LEFT_MOTOR_PDO_ID 0x201
#define RIGHT_MOTOR_PDO_ID 0x202


#define DISTANCE_WHEELS  0.562
#define DIAMETER_WHEELS    0.169
#define PI 3.1415926
#define EXP_DOUBLE 1e-6

bool flag_send = false;
ros::Publisher roscan_send_message;

void setCANopenMotorToward(unsigned int ID, uint8_t toward)
{
    ROS_INFO("Ready to send can_message : motor toward");
    setlocale(LC_ALL, "");

    uint8_t transition[8];
    can_msgs::Frame can_frame;
    memset(&transition, 0, sizeof(transition));

    //只发送一个字节0x2F 发送两个字节0x2B 发送四个字节0x23 
    transition[0] = 0x2F;

    //设置电机方向的索引为607E低位在前高位在后
    transition[1] = 0x7E;
    transition[2] = 0x60;

    //子索引为00
    transition[3] = 0x00;

    //设置方向，低位在前高位在后

    transition[4] = toward;
    transition[5] = 0x00;
    transition[6] = 0x00;
    transition[7] = 0x00;

    can_frame.id = ID;

    can_frame.is_extended = true;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 8; //有效数据长度

    for (uint8_t i = 0; i < can_frame.dlc; i++)
    {

        can_frame.data[i] = transition[i];
    }

    roscan_send_message.publish(can_frame);

    ROS_INFO("send messages succeed : work model shift");
}

void setCANopenMotorSpeed(unsigned int ID, double speed)
{
    bool flag_sign = false;
    
    if(speed < 0)
    {
        flag_sign = true;
    }

    double rpm = fabs(speed);
    int encoder_resolution = 10000;
    int dec = rpm *512* encoder_resolution/1875;

    if(flag_sign == true)
    {
        dec = (~dec) + 1;
    }

    ROS_INFO("Ready to send can_message : motor speed  0x%X" , dec);
    setlocale(LC_ALL, "");

    uint8_t transition[8];
    can_msgs::Frame can_frame;
    memset(&transition, 0, sizeof(transition));

    //设置轮子转速
    unsigned int encode_tmp = 0xff;
    transition[0] = dec&encode_tmp;
    transition[1] = (dec>>8)&encode_tmp;
    transition[2] = (dec>>16)&encode_tmp;
    transition[3] = (dec>>24)&encode_tmp;
 
    can_frame.id = ID;

    can_frame.is_extended = true;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 4; //有效数据长度

    for (uint8_t i = 0; i < can_frame.dlc; i++)
    {

        can_frame.data[i] = transition[i];
    }

    roscan_send_message.publish(can_frame);

    ROS_INFO("send messages succeed : motor speed %X %X %X %X",transition[0],transition[1],transition[2],transition[3]);
}

void setCANopenMotorWorkMdl(unsigned int ID , uint8_t work_mdl)
{

    ROS_INFO("Ready to send can_message : work model shift");
    setlocale(LC_ALL, "");

    uint8_t transition[8];
    can_msgs::Frame can_frame;
    memset(&transition, 0, sizeof(transition));

    //只发送一个字节0x2F 发送两个字节0x2B 发送四个字节0x23
    transition[0] = 0x2F;

    //设置工作模式的索引为6060低位在前高位在后
    transition[1] = 0x60;
    transition[2] = 0x60;

    //子索引为00
    transition[3] = 0x00;

    //位置模式是0 速度模式是3
    transition[4] = work_mdl;

    //剩下全部补0即可
    transition[5] = 0x00;
    transition[6] = 0x00;
    transition[7] = 0x00;

    can_frame.id = ID;

    can_frame.is_extended = true;
    can_frame.is_rtr = false;
    can_frame.is_error = false;
    can_frame.dlc = 8; //有效数据长度

    for (uint8_t i = 0; i < can_frame.dlc; i++)
    {

        can_frame.data[i] = transition[i];
    }

    roscan_send_message.publish(can_frame);

    ROS_INFO("send messages succeed : work model shift");

}

 

void WorkMdlCallback(const std_msgs::Int8 & msg)
{
    ROS_INFO("work model setting : %d" ,msg.data);
    setCANopenMotorWorkMdl(LEFT_MOTOR_CAN_ID,static_cast<uint8_t>(msg.data));
    setCANopenMotorWorkMdl(RIGHT_MOTOR_CAN_ID,static_cast<uint8_t>(msg.data));
    //send_can_message();
}

void MotorSpeedCallback(const std_msgs::Float32MultiArray & msg)
{
    ROS_INFO("motor speed setting : %f" ,msg.data[1]);
    //如果是左轮
    unsigned int id;
    uint8_t dir;

    if(msg.data.size() == 2)
    {
        if(msg.data[0] < 0)
        {
            ROS_INFO("motor speed setting : left wheel");
            id = LEFT_MOTOR_PDO_ID;
        }
        //如果是右轮
        else if(msg.data[0] > 0)
        {
            ROS_INFO("motor speed setting : right wheel");
            id = RIGHT_MOTOR_PDO_ID;
        }
        setCANopenMotorSpeed(id,msg.data[1]);
    }
    else if(msg.data.size() == 3)
    {
        id = LEFT_MOTOR_PDO_ID;
        setCANopenMotorSpeed(id,msg.data[1]);
        id = RIGHT_MOTOR_PDO_ID;
        setCANopenMotorSpeed(id,msg.data[2]);
    }

}

void cmdVelCallback(const geometry_msgs::Twist & msg)
{
//    vel_left = (curr_cmd.lin - curr_cmd.ang * L/ 2.0) / left_wheel_radius;
//    vel_right = (curr_cmd.lin + curr_cmd.ang * L/ 2.0) / right_wheel_radius;
//    m/s
    double v_line_x = msg.linear.x;
    double v_angr_z = msg.angular.z;
    double radius = DIAMETER_WHEELS / 2;
    double dist_w = DISTANCE_WHEELS;

    double vel_left = (v_line_x - v_angr_z * dist_w / 2.0) / radius;  // m/s
    double vel_right = (v_line_x + v_angr_z * dist_w / 2.0) / radius;

    double speed_left = vel_left * 2 * PI;     // rad/s
    double speed_right = vel_right * 2 * PI;

    speed_left *= 9.55; //rpm
    speed_right *= 9.55;

    unsigned int id;
    id = LEFT_MOTOR_PDO_ID;
    setCANopenMotorSpeed(id,speed_left);
    id = RIGHT_MOTOR_PDO_ID;
    setCANopenMotorSpeed(id,-speed_right);

}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "can_sender");
    ros::NodeHandle n("~");

    ros::Subscriber work_mdl_sub = n.subscribe("/work_mdl_shift" , 60 , WorkMdlCallback);
    ros::Subscriber motor_speed_sub = n.subscribe("/motor_speed" , 60 , MotorSpeedCallback);

    ros::Subscriber cmd_vel_sub = n.subscribe("/cmd_vel" , 60 , cmdVelCallback);

    roscan_send_message = n.advertise<can_msgs::Frame>("/sent_messages", 100);

    ros::spin();

}