【四足】ROS1到ROS2的C++代码迁移教程

以下代码以Aonemotor.cpp为例,ROS2有很多命名规范不能用数字和下划线

1、ROS1

a1motor1.cpp

#include <stdio.h>
#include <errno.h>      //错误定义
#include <string.h>
#include <unistd.h>    //Unix标准函数定义, usleep()
#include <sys/time.h> 
#include "LSerial.h"    //串口通信函数
#include "motor_ctrl.h" //声明发送数据、接收数据的结构体，以及函数声明
#include "check.h"


MOTOR_send motor_s, motor_s1;
MOTOR_recv motor_r;


int fd=open_set((char*)"/dev/leg1");


int main(int argc, char** argv)
{
        //初始化ros节点
    ros::init(argc, argv, "a1motor_1_cpp");
        //创建节点句柄
    ros::NodeHandle nh;
  
    A1MotorData_1_sub = nh.subscribe("A1Motor_1_data", 16, A1Motor_1_Callback);
    A1MotorBack_1_pub = nh.advertise<lys_pkg::A1MotorBack_msg>("A1MotorBack_1_data",10); 

    ros::spin();
    return 0;
}

void A1Motor_1_Callback(const lys_pkg::A1Motor_msg& msg)
{
    motor_s.id = msg.id;
    motor_s.mode = 10;
    motor_s.T = msg.T;            //单位：Nm, T<255.9
    motor_s.W = msg.W;//21.0;               //单位：rad/s, W<511.9
    motor_s.Pos = msg.Pos;          //单位：rad, Pos<131071.9
    motor_s.K_P = msg.P;        //K_P<31.9
    motor_s.K_W = msg.D;//10;        //K_W<63.9
    modify_data(&motor_s);
    int sta;
    if(motor_s.T == 0 && motor_s.W == 0 && motor_s.Pos == 0 && motor_s.K_P == 0 && motor_s.K_W ==3)
    {
        
        send_recv(fd, &motor_s, &motor_r);
        extract_data(&motor_r);
        if(motor_s.id==0)
        {
            A1MotorcaliPos_[0]=motor_r.Pos;
            std::cout<<"第一条腿ID为0的电机初始角为 ="<<A1MotorcaliPos_[0]<<std::endl;
            a1motorback_1_msg.Pos = motor_r.Pos;
            a1motorback_1_msg.id = motor_s.id;
            A1MotorBack_1_pub.publish(a1motorback_1_msg);
        } 
        else if(motor_s.id==1)
        {
            A1MotorcaliPos_[1]=motor_r.Pos;
            std::cout<<"第一条腿ID为1的电机初始角为 ="<<A1MotorcaliPos_[1]<<std::endl;
            a1motorback_1_msg.Pos = motor_r.Pos;
            a1motorback_1_msg.id = motor_s.id;
            A1MotorBack_1_pub.publish(a1motorback_1_msg);
        }
    }
    else
    {
        send_recv(fd, &motor_s, &motor_r);
        //extract_data(&motor_r);
        //printf("第一条腿电机反馈角:%f\n",motor_r.Pos);
          
    }
  /*  close_serial(fd); */
}

check.h

#include "ros/ros.h"
#include "lys_pkg/A1MotorBack_msg.h"
#include "lys_pkg/A1Motor_msg.h"

ros::Publisher A1MotorBack_1_pub;
ros::Publisher A1MotorBack_2_pub;
ros::Publisher A1MotorBack_3_pub;
ros::Publisher A1MotorBack_4_pub; 

ros::Subscriber A1MotorData_1_sub;
ros::Subscriber A1MotorData_2_sub;
ros::Subscriber A1MotorData_3_sub;
ros::Subscriber A1MotorData_4_sub;

lys_pkg::A1MotorBack_msg  a1motorback_1_msg;
lys_pkg::A1MotorBack_msg  a1motorback_2_msg;
lys_pkg::A1MotorBack_msg  a1motorback_3_msg;
lys_pkg::A1MotorBack_msg  a1motorback_4_msg;


void A1Motor_1_Callback(const lys_pkg::A1Motor_msg& msg);
void A1Motor_2_Callback(const lys_pkg::A1Motor_msg& msg);
void A1Motor_3_Callback(const lys_pkg::A1Motor_msg& msg);
void A1Motor_4_Callback(const lys_pkg::A1Motor_msg& msg);

volatile float A1MotorcaliPos_[8];//calibration angle

2、ROS2

Aonemotor.cpp

#include <stdio.h>
#include <errno.h>      //错误定义
#include <string.h>
#include <unistd.h>    //Unix标准函数定义, usleep()
#include <sys/time.h> 
#include "LSerial.h"    //串口通信函数
#include "motor_ctrl.h" //声明发送数据、接收数据的结构体，以及函数声明
#include "check.h"


MOTOR_send motor_s, motor_s1;
MOTOR_recv motor_r;


int fd=open_set((char*)"/dev/leg1");
void AoneMotorCallback(const test_msgs::msg::Motormsg::SharedPtr msg);

int main(int argc, char** argv)
{
        //初始化ros节点
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("a1motor_1_cpp");
    // auto node = std::make_shared<rclcpp::Node>("a1motor_1_cpp");
        //创建节点句柄
    // ros::NodeHandle nh;
  
    // A1MotorData_1_sub = nh.subscribe("A1Motor_1_data", 16, A1Motor_1_Callback);
    // A1MotorBack_1_pub = nh.advertise<lys_pkg::A1MotorBack_msg>("A1MotorBack_1_data",10); 

    std::cout<<"A1Motor1"<<std::endl;
    AoneDatasub  = node->create_subscription<test_msgs::msg::Motormsg>("/A1Motor_1_data",16,AoneMotorCallback);
    AoneBackpub = node->create_publisher<test_msgs::msg::MotorBackmsg>("/A1MotorBack_1_data",10);

    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}

void AoneMotorCallback(const test_msgs::msg::Motormsg::SharedPtr msg)
{
    std::cout<<"A1Motor2"<<std::endl;
    motor_s.id = msg->id;
    motor_s.mode = 10;
    motor_s.T = msg->t;            //单位：Nm, T<255.9
    motor_s.W = msg->w;//21.0;               //单位：rad/s, W<511.9
    motor_s.Pos = msg->pos;          //单位：rad, Pos<131071.9
    motor_s.K_P = msg->p;        //K_P<31.9
    motor_s.K_W = msg->d;//10;        //K_W<63.9
    modify_data(&motor_s);
    int sta;
    if(motor_s.T == 0 && motor_s.W == 0 && motor_s.Pos == 0 && motor_s.K_P == 0 && motor_s.K_W ==3)
    {
        
        send_recv(fd, &motor_s, &motor_r);
        extract_data(&motor_r);
        if(motor_s.id==0)
        {
            A1MotorcaliPos_[0]=motor_r.Pos;
            std::cout<<"第一条腿ID为0的电机初始角为 ="<<A1MotorcaliPos_[0]<<std::endl;
            Aonebackmsg.pos = motor_r.Pos;
            Aonebackmsg.id = motor_s.id;
            AoneBackpub->publish(Aonebackmsg);
            
        } 
        else if(motor_s.id==1)
        {
            A1MotorcaliPos_[1]=motor_r.Pos;
            std::cout<<"第一条腿ID为1的电机初始角为 ="<<A1MotorcaliPos_[1]<<std::endl;
            Aonebackmsg.pos = motor_r.Pos;
            Aonebackmsg.id = motor_s.id;
            AoneBackpub->publish(Aonebackmsg);
        }
    }
    else
    {
        send_recv(fd, &motor_s, &motor_r);
        //extract_data(&motor_r);
        //printf("第一条腿电机反馈角:%f\n",motor_r.Pos);
          
    }
  /*  close_serial(fd); */
}

check.h

#include "rclcpp/rclcpp.hpp"
#include "test_msgs/msg/motor_backmsg.hpp"
#include "test_msgs/msg/motormsg.hpp"

rclcpp::Publisher<test_msgs::msg::MotorBackmsg>::SharedPtr AoneBackpub;
rclcpp::Publisher<test_msgs::msg::MotorBackmsg>::SharedPtr AtwoBackpub;
rclcpp::Publisher<test_msgs::msg::MotorBackmsg>::SharedPtr AthreeBackpub;
rclcpp::Publisher<test_msgs::msg::MotorBackmsg>::SharedPtr AfourBackpub;
// rclcpp::Publisher<std_msgs::msg::String>::SharedPtr MotorBack_pub;
// rclcpp::Publisher A1MotorBack_2_pub;
// rclcpp::Publisher A1MotorBack_3_pub;
// rclcpp::Publisher A1MotorBack_4_pub; 

rclcpp::Subscription<test_msgs::msg::Motormsg>::SharedPtr AoneDatasub;
rclcpp::Subscription<test_msgs::msg::Motormsg>::SharedPtr AtwoDatasub;
rclcpp::Subscription<test_msgs::msg::Motormsg>::SharedPtr AthreeDatasub;
rclcpp::Subscription<test_msgs::msg::Motormsg>::SharedPtr AfourDatasub;
// rclcpp::Subscriber A1MotorData_2_sub;
// rclcpp::Subscriber A1MotorData_3_sub;
// rclcpp::Subscriber A1MotorData_4_sub;


test_msgs::msg::MotorBackmsg  Aonebackmsg;
test_msgs::msg::MotorBackmsg  Atwobackmsg;
test_msgs::msg::MotorBackmsg  Athreebackmsg;
test_msgs::msg::MotorBackmsg  Afourbackmsg;
// lys_pkg::A1MotorBack_msg  a1motorback_2_msg;
// lys_pkg::A1MotorBack_msg  a1motorback_3_msg;
// lys_pkg::A1MotorBack_msg  a1motorback_4_msg;


void AoneMotorCallback(const test_msgs::msg::Motormsg::SharedPtr msg);
void AtwoMotorCallback(const test_msgs::msg::Motormsg::SharedPtr msg);
void AthreeMotorCallback(const test_msgs::msg::Motormsg::SharedPtr msg);
void AfourMotorCallback(const test_msgs::msg::Motormsg::SharedPtr msg);
// void A1Motor_2_Callback(const lys_pkg::A1Motor_msg& msg);
// void A1Motor_3_Callback(const lys_pkg::A1Motor_msg& msg);
// void A1Motor_4_Callback(const lys_pkg::A1Motor_msg& msg);

volatile float A1MotorcaliPos_[8];//calibration angle

3、修改语句说明

1、check.h
修改头文件
#include “ros/ros.h” 改为 #include “rclcpp/rclcpp.hpp”
#include “lys_pkg/A1Motor_msg.h” 改为 #include “test_msgs/msg/motormsg.hpp”
#include “lys_pkg/A1MotorBack_msg.h” 改为 #include “test_msgs/msg/motor_backmsg.hpp”
修改发布者
ros::Publisher A1MotorBack_1_pub; 改为 rclcpp::Publisher<test_msgs::msg::MotorBackmsg>::SharedPtr AoneBackpub;
修改接收者
ros::Subscriber A1MotorData_1_sub; 改为 rclcpp::Subscription<test_msgs::msg::Motormsg>::SharedPtr AoneDatasub;
修改消息类型
lys_pkg::A1MotorBack_msg a1motorback_1_msg; 改为 test_msgs::msg::MotorBackmsg Aonebackmsg;
A1MotorcaliPos_[8]不用改
volatile float A1MotorcaliPos_[8];

2、a1motor.cpp
修改源文件名称
a1motor.cpp 改为 Aonemotor.cpp
初始化ros节点
ros::init(argc, argv, “a1motor_1_cpp”); 改为 rclcpp::init(argc, argv);
创建节点句柄改为创建节点
ros::NodeHandle nh; 改为 auto node = rclcpp::Node::make_shared(“a1motor_1_cpp”);
修改发布者
#include “lys_pkg/A1MotorBack_msg.h” 改为 #include “test_msgs/msg/motor_backmsg.hpp”
修改接收者
A1MotorData_1_sub = nh.subscribe(“A1Motor_1_data”, 16, A1Motor_1_Callback); 改为 AoneDatasub = node->create_subscription<test_msgs::msg::Motormsg>(“/A1Motor_1_data”,16,AoneMotorCallback);
修改回调函数
void A1Motor_1_Callback(const lys_pkg::A1Motor_msg& msg) 改为 void AoneMotorCallback(const test_msgs::msg::Motormsg::SharedPtr msg)
修改msg
msg.id 改为 msg->id
msg.T 改为 msg->t
msg.W 改为 msg->w
msg.Pos 改为 msg->pos
msg.K_P 改为 msg->p
msg.K_W 改为 msg->w
a1motorback_1_msg 改为 Aonebackmsg
修改发布者发布消息
A1MotorBack_1_pub.publish(a1motorback_1_msg); 改为 AoneBackpub->publish(Aonebackmsg);

3、CMakeLists.txt
添加可执行文件
add_executable(Aonemotor src/Aonemotor.cpp)
ament_target_dependencies(Aonemotor rclcpp std_msgs test_msgs)
添加链接库
target_link_libraries(Aonemotor libUnitree_motor_SDK_Linux64.so )
添加install
install(TARGETS
Aonemotor
DESTINATION lib/${PROJECT_NAME})