3
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 1 year has passed since last update.

RobohanAdvent Calendar 2021

Day 20

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

Last updated at Posted at 2021-12-19

前編はこちら↓

#目次
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);
}

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

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

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

##マイコン
Mbed.png
こんな感じでデータの送信と受信を同時に行えています。

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

#参考文献

3
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
3
2

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?