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

前編はこちら↓

https://qiita.com/hbvcg00/items/552fb765530ebea3e4d8

#目次
[1.ROSと通信する](#rosと通信する)
[2.実践](#実践)
[3.おわりに](#おわりに)

# ROSと通信する
今回はROS 1 Noeticで行います。

## 送信側

```C++: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型のデータを投げるとそれをマイコンに投げます。


##受信側

```C++: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);
}
```
データを受け取るとそれを表示するというのを無限ループで繰り返します。

#実践
##送信
![ros_send.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2017430/cbdc6d4c-2b29-b9af-9db0-c2322c869a81.png)
上半分でudp_sendノードを実行しています。
下半分で/sendDataにメッセージを配信しています。

##受信
![ros_receive.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2017430/5db371db-d322-3fdd-3619-44ed84bc6aec.png)
マイコンからのメッセージを受信できています。

##マイコン
![Mbed.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/2017430/4917464a-95ae-6291-12ff-f47e8ff3ba87.png)
こんな感じでデータの送信と受信を同時に行えています。

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


#参考文献

https://qiita.com/srs/items/c9286b5cff99e741b993