基于ros_control使用真实硬件odrive搭建两轮差分小车

1.硬件及结构件

2.标定电机

参考：
标定电机，配置can接口

3.ros_control

参考
ros control介绍博客
ros control代码 GIT

1. 配置车体参数

2. 配置手柄

3. ros control 模板 结构说明

代码：main.cpp

#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include <dynamic_reconfigure/server.h>
#include "odrive_diff.h"

int main(int argc, char **argv) {
    ros::init(argc, argv, "odrive_driver");
    
    // Create and initialize driver
    odrive_diff robot; //初始化 差分小车控制对象  TODO： 需要自定义
    printf("Create and initialize driver.\n");

    controller_manager::ControllerManager cm(&robot); //控制器对象 
    printf("Create and initialize controller_manager.\n");
    
    ros::AsyncSpinner spinner(1);
    spinner.start();

    ros::Time prev_time = ros::Time::now();
    ros::Rate rate(50.0);

    while (ros::ok()) {
        const ros::Time time = ros::Time::now();
        const ros::Duration period = time - prev_time;
        robot.read(); // TODO:需要自定义
        cm.update(time, period);
        robot.write(); // TODO:需要自定义
        rate.sleep();
        //robot.updateWD(); // TODO:喂狗 需要自定义
    }
    return 0;
}

从模板里可以看出只要对robot类进行相应的编写即可，这个类需要重写read函数、write函数、updateWD函数。

代码：odrive_diff.h中编写底盘控制指令的代码即可

#include <controller_manager/controller_manager.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

using namespace odrive_can_ros;

class odrive_diff : public hardware_interface::RobotHW {
public:
    odrive_diff(); 
    ~odrive_diff();
    void read(); 
    void write();
    void updateWD();

private:
    hardware_interface::JointStateInterface joint_state_interface;
    hardware_interface::VelocityJointInterface velocity_joint_interface;
    // The units for wheels are radians (pos), radians per second (vel,cmd), and Netwton metres (eff)
    //单位
    struct Joint {
        std_msgs::Float64 pos; //反馈的转
        std_msgs::Float64 vel; //反馈的速度
        std_msgs::Float64 eff; //力矩
        std_msgs::Float64 cmd; //速度指令
    } joints[2];  //对于控制器
    /******************************/
    // ODRIVE CAN SETUP VARIABLES //
    CANSimple master; //CAN解析的程序
    /******************************/
    ros::Time current_time;
    ros::Time last_read;
    ros::Time startTime;
};

继承 hardware_interface::RobotHW类，重写其中的发送控制函数以及读取反馈的函数即可实现控制；
odrive_diff.cpp中编写底盘控制指令的代码即可

odrive_diff::odrive_diff()
{ 
  hardware_interface::JointStateHandle left_wheel_state_handle(
    "left_wheel", &joints[0].pos.data, &joints[0].vel.data, &joints[0].eff.data);

  hardware_interface::JointStateHandle right_wheel_state_handle(
    "right_wheel", &joints[1].pos.data, &joints[1].vel.data, &joints[1].eff.data);

  joint_state_interface.registerHandle(left_wheel_state_handle);
  joint_state_interface.registerHandle(right_wheel_state_handle);
  registerInterface(&joint_state_interface);

  hardware_interface::JointHandle left_wheel_vel_handle(
    joint_state_interface.getHandle("left_wheel"), &joints[0].cmd.data);

  hardware_interface::JointHandle right_wheel_vel_handle(
    joint_state_interface.getHandle("right_wheel"), &joints[1].cmd.data);
W
  velocity_joint_interface.registerHandle(left_wheel_vel_handle);
  velocity_joint_interface.registerHandle(right_wheel_vel_handle);
  registerInterface(&velocity_joint_interface);
  /****** ODRIVE CAN SETUP *******/
    master.init();
}

odrive_diff::~odrive_diff()
{
  // Set velocity 关闭设置速度为0 
}

void odrive_diff::read()
{
  /****** ODRIVE CAN Collect data 读取电机驱动器数据 *******/
  vel0 = left_vel;
  vel1 = right_vel;
  pos0 = left_pos;
  pos1 = right_pos;
  //printf("****************robot in read FINSH wheel line vel；反馈的速度添到vel**********************.\n");
  joints[0].vel.data = -1*DIRECTION_CORRECTION * (vel0) * 2*PI*wheel_radius;   
  joints[1].vel.data = DIRECTION_CORRECTION * (vel1) * 2*PI*wheel_radius;

  joints[0].pos.data = -1*DIRECTION_CORRECTION * (pos0) * 2*PI*wheel_radius;
  joints[1].pos.data = DIRECTION_CORRECTION * (pos1) * 2*PI*wheel_radius;
}

void odrive_diff::write()
{
  double left_speed =   -1*DIRECTION_CORRECTION * (joints[0].cmd.data); 
  double right_speed =    DIRECTION_CORRECTION * (joints[1].cmd.data) ;
  /* 设置控制 左右轮速 joints[*].cmd.data */
}

void odrive_diff::updateWD(void){
  // update watchdog
}

4.odrive协议替换

can 教程

使用odrive 驱动 解析can数据

5.实车测试