基于ROS的工控机与Speedgoat通讯链路实现（工控机端）

原创已于 2026-03-27 17:26:36 修改 · 397 阅读

3 ·

本内容遵循CC 4.0 BY-SA版权协议

GEO检测

于 2026-03-27 17:19:20 首次发布

文章目录

前言
一、创建工作空间
二、配置Ubuntu的IP地址
三、编写代码并运行
总结

前言

准备环境：Ubuntu20.04，ROS Noetic，Visual Studio Code
核心链路：ROS_Node1 → ROS_Node2（同时兼任UDP_Node1，UDP发送端） → 以太网UDP传输 → UDP_Node2（Speedgoat 端，UDP接收端）

一、创建工作空间

1. 初始化

打开ubuntu终端，输入：

mkdir -p speedgoat_ws/src
cd speedgoat_ws
catkin_make

2. 进入src创建ros包(two_way)并添加依赖

cd src
catkin_create_pkg two_way roscpp rospy std_msgs

3. 创建代码文件并配置CMakeLists.txt

在two_way下的src文件夹中创建two_pub.cpp和two_sub.cpp，并写入：

#include "ros/ros.h"

int main(int argc, char *argv[])
{
	return 0;
}

修改two_way下的CMakeLists.txt，添加可执行文件和链接库配置：
在终端输入catkin_make后回车编译，应该无报错

二、配置Ubuntu的IP地址

用网线连接Speedgoat（ETH 1）和工控机（网口），打开设置 → 网络 → IPv4 → 手动，填写地址和子网掩码，最后点击“应用”
拔掉工控机网线，等待数秒重新插回，查看“详细信息”，确认IP地址修改成功
在ubuntu终端中输入ping 192.168.8.1，测试与Speeedgoat的网络连通性（无需反向ping测试）

三、编写代码并运行

1. 编写two_pub.cpp

该节点负责发布数据，并订阅Speedgoat返回的反馈数据：

#include "ros/ros.h"
#include "std_msgs/Int32.h"

// 接收UDP返回的数据
void doFeedback(const std_msgs::Int32::ConstPtr& msg)
{
    ROS_INFO("***这里是ROS_Node1，收到Speedgoat返回：%d***", msg->data);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "ros_node1");
    ros::NodeHandle nh;
    // 发布话题chatter
    ros::Publisher pub = nh.advertise<std_msgs::Int32>("chatter", 10);
    // 订阅反馈话题
    ros::Subscriber sub = nh.subscribe<std_msgs::Int32>("udp_feedback", 10, doFeedback);
    
    ros::Rate r(1); // 1秒发布1次
    int count = 1;
    while (ros::ok())
    {
        std_msgs::Int32 msg;
        msg.data = count;
        pub.publish(msg);
        ROS_INFO("这里是ROS_Node1：%d", count);
        
        ros::spinOnce();
        r.sleep();
        count++;
    }
    
    return 0;
}

2. 编写two_sub.cpp

该节点负责通过udp与Speedgoat通信，并将返回数据发布至ros话题：

#include "ros/ros.h"
#include "std_msgs/Int32.h"
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <cstring>
#include <unistd.h>
#include <thread>

#define TARGET_IP "192.168.8.1"  // 目标IP地址
#define UDP_PORT 8001
#define LOCAL_UDP_PORT 9001  // 本地接收端口

int udp_fd;
struct sockaddr_in target_addr;
struct sockaddr_in local_addr; 
ros::Publisher pub_feedback; 

// UDP接收线程函数
void udpRecvThread() 
{
    char recv_buf[4];
    struct sockaddr_in send_addr;
    while (ros::ok()) 
    {
        socklen_t send_len = sizeof(send_addr);
        ssize_t recv_len = recvfrom(udp_fd, recv_buf, sizeof(recv_buf), 0,
                                    (struct sockaddr *)&send_addr, &send_len);
        if (recv_len > 0) 
        {
            uint32_t recv_data = ntohl(*(uint32_t *)recv_buf);
            ROS_INFO("***这里是ROS_Node2/UDP1，收到UDP2返回：%d***", recv_data);
            
            std_msgs::Int32 feedback_msg;
            feedback_msg.data = recv_data;
            pub_feedback.publish(feedback_msg);
        }
    }
}

void doMsg(const std_msgs::Int32::ConstPtr& msg)
{
    uint32_t send_data = htonl(msg->data);
    
    // UDP发送
    sendto(udp_fd, &send_data, sizeof(send_data), 0,
           (struct sockaddr *)&target_addr, sizeof(target_addr));
    
    ROS_INFO("这里是ROS_Node2/UDP1，向UDP2发送：%d", msg->data);
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL, "");
    ros::init(argc, argv, "ros_node2_to_udp2");
    ros::NodeHandle nh;
    
    // 初始化反馈数据发布者
    pub_feedback = nh.advertise<std_msgs::Int32>("udp_feedback", 10);

    // 初始化UDP socket
    udp_fd = socket(AF_INET, SOCK_DGRAM, 0);

    // 初始化 target_addr
    memset(&target_addr, 0, sizeof(target_addr));
    target_addr.sin_family = AF_INET;
    target_addr.sin_port = htons(UDP_PORT);
    inet_pton(AF_INET, TARGET_IP, &target_addr.sin_addr);
    
    // 初始化 local_addr
    memset(&local_addr, 0, sizeof(local_addr));
    local_addr.sin_family = AF_INET;
    local_addr.sin_port = htons(LOCAL_UDP_PORT);
    local_addr.sin_addr.s_addr = htonl(INADDR_ANY);
    bind(udp_fd, (struct sockaddr *)&local_addr, sizeof(local_addr));

    // 启动接收线程
    std::thread recv_thread(udpRecvThread);
    recv_thread.detach();

    ros::Subscriber sub = nh.subscribe<std_msgs::Int32>("chatter", 10, doMsg);
    ros::spin();
    close(udp_fd);

    return 0;
}

3. 运行程序

打开ubuntu终端，输入roscore启动ros

打开vscode终端1，运行two_pub节点：

source ./devel/setup.bash
rosrun two_way two_pub

打开vscode终端2，运行two_sub节点：

source ./devel/setup.bash
rosrun two_way two_sub

程序成功运行（发送）
程序成功运行（接收）

总结

在测试过程中一定要多和Speedgoat端多沟通，确保IP地址、端口号一致，以及对齐进度。

标签

#ubuntu #机器人 #经验分享