前編はこちら↓
#目次
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通信について書きました。
イーサネットを使って通信している例は意外と少ないので、参考になれば幸いです。
#参考文献