Mbed OS 6上でNUCLEO-F767ZIでUDP通信を実装し、ROSと通信する（後編）

前編はこちら↓

目次

1.ROSと通信する
2.実践
3.おわりに

ROSと通信する

今回はROS 1 Noeticで行います。

送信側

udp_send.cpp

#include <arpa/inet.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <string>
#include "std_msgs/String.h"
#include <ros/ros.h>

struct sockaddr_in addr;
int udp;

void subscribeMsgCb(const std_msgs::String &msg);

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "udp_send");
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("sendData", 10, subscribeMsgCb);

    // マイコンのIPを入れる
    const std::string address = "127.0.0.1";
    // マイコンのPortを入れる
    const int port = 5000;

    memset((char *)&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(address.c_str());
    addr.sin_port = htons(port);

    if ((udp = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
    {
        ROS_ERROR("error");
    }
    else
    {
        ROS_INFO("socket open");
    }

    ros::spin();

    close(udp);

    return 0;
}

void subscribeMsgCb(const std_msgs::String &msg)
{
    ROS_INFO("send : %s", msg.data.c_str());

    if (sendto(udp, msg.data.c_str(), msg.data.length(), 0, (struct sockaddr *)&addr, sizeof(addr)) < 0)
    {
        ROS_ERROR("send error");
    }
}

sendDataというtopicに適当にstd_msgs::String型のデータを投げるとそれをマイコンに投げます。

受信側

udp_receive.cpp

#include <arpa/inet.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <string>
#include <ros/ros.h>

struct sockaddr_in addr;
int udp;

void receive();

int main(int argc, char *argv[])
{
    ros::init(argc, argv, "udp_receive");
    ros::NodeHandle nh;

    // PCのIPを入れる
    const std::string address = "0.0.0.0";
    // PCのPortを入れる
    const int port = 4000;

    memset((char *)&addr, 0, sizeof(addr));
    addr.sin_family = AF_INET;
    addr.sin_addr.s_addr = inet_addr(address.c_str());
    addr.sin_port = htons(port);

    if ((udp = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
    {
        ROS_ERROR("error");
    }
    else
    {
        ROS_INFO("socket open");
    }

    bind(udp, (const struct sockaddr *)&addr, sizeof(addr));

    while (ros::ok())
    {
        receive();
    }

    close(udp);

    return 0;
}

void receive()
{
    char buffer[256];
    memset(buffer, 0, sizeof(buffer));
    recv(udp, buffer, sizeof(buffer), 0);
    ROS_INFO("received:%s", buffer);
}

データを受け取るとそれを表示するというのを無限ループで繰り返します。

実践

送信

上半分でudp_sendノードを実行しています。
下半分で/sendDataにメッセージを配信しています。

受信

マイコンからのメッセージを受信できています。

マイコン

こんな感じでデータの送信と受信を同時に行えています。

おわりに

本記事では前編、後編に渡ってマイコンとROSのUDP通信について書きました。
イーサネットを使って通信している例は意外と少ないので、参考になれば幸いです。

参考文献