ROS 2 (micro-RTPS) サンプル

eSOLさんが動作確認してくださったサンプルを貼り付けます。

※2018/10/01追記：やればやるほどハマるROS 2ですが、自分のRaspberry Pi 3にUbuntu Mate、micro-RTPS-Agentを入れてGR-ROSEをつなげてみましたので手順を追記しました。実際のところ、ClientであるPublisherとSubscriberの両方がいないと動作確認できないため、ROS 2はかなり準備の敷居が高いなと思う今日この頃です。

●Rapberry Pi 3 にmicroRTPS-Agentを入れる手順

(Windows PCで実施します)

Ubuntu Mateを入手
https://ubuntu-mate.org/raspberry-pi/ubuntu-mate-16.04.2-desktop-armhf-raspberry-pi.img.xz
ダウンロードファイルを解凍しWin32DiskImagerでマイクロSDへWrite

(RaspberryPi 3を立ち上げUbuntuで実施します)
ApplicationsからMate Terminalを起動

(Gitをインストール)
sudo apt-get update
sudo apt-get install git-core
git --version

(RTPSをインストール)
sudo git clone --recursive https://github.com/eProsima/micro-RTPS.git
cd micro-RTPS
sudo mkdir build

sudo apt-get install cmake
sudo cmake -DTHIRDPARTY=ON ..
sudo make
sudo make install

(eProsimaのRTPSで、足りないTranspote?があるらしいので一旦Agentをアンインストール)
sudo rm -r micro-RTPS-agent

(eSOLさんで確認済みのちょっと古いmicro-RTPS-Agentをインストール)
sudo git clone https://github.com/GadgetRenesas/micro-RTPS-agent
cd micro-RTPS-agent
sudo mkdir build
cd build
sudo cmake -DTHIRDPARTY=ON ..
sudo make

(Agentを起動、Clientは後から起動する必要あります)

Ethernetの設定で固定IPとして192.168.1.101とする。サブネットマスクは255.255.255.0。

cd micro-RTPS-agent/build
./MicroRTPSAgent udp 2020

(GR-ROSE側のプログラム)

●micro-RTPCをsubscriberとしたときの動作確認

基本的には固定IPでの通信。デフォルトのAgent IPアドレスは192.168.1.101。GR-ROSE(Client)のIPアドレスは192.168.1.123。V0.03のテンプレートでは./freertos/config_files/FreeRTOSConfig.hで設定しているが、ゆくゆくはArduinoライクにsketch.cppで設定できるようにしたいと思ってます。

#include <Arduino.h>
extern "C" {
#include "FreeRTOS.h"
#include "task.h"
#include <stdio.h>
#include "micrortps_demo_config.h"
#include "HelloWorld.h"
}

#ifdef demoConfigMICRORTPS_SELECT_SUBSCRIBER
int32_t g_loop_out = 0;
#define swValidate_Connect

int g_subscribe_cnt = 0; /* temporary until support "printf" */
char g_last_rcv_buf[80]; /* temporary until support "printf" */


#define HELLO_WORLD_TOPIC 0x01

static void prvMicrortpsSub(void * pvParameters);

#ifdef demoConfigMICRORTPS_USE_UDP
static const uint8_t agentIPAddress[4] =
{
 democonfigMICRORTPS_AGENT_ADDR0,
 democonfigMICRORTPS_AGENT_ADDR1,
 democonfigMICRORTPS_AGENT_ADDR2,
 democonfigMICRORTPS_AGENT_ADDR3
};
static const uint16_t agentPort = democonfigMICRORTPS_AGENT_PORT;
#endif


void check_and_print_error(Session* session, const char* where)
{
 if(session->last_status_received)
 {
 if(session->last_status.status == STATUS_OK)
 {
 return; //All things go well
 }
 else
 {
 configPRINTF( ("Status error (%i)", session->last_status.status) );
 }
 }
 else
 {
 configPRINTF( ("Connection error") );
 }
 configPRINTF( (" at %s\n", where) );

 /*TODO: When the connection is failed, client should close
 * session and reconnect to agent(not implemented yet). */
 return;
}

void on_topic(ObjectId id, MicroBuffer* serialized_topic, void* args)
{
 (void) args;
 if(HELLO_WORLD_TOPIC == id.data[0])
 {
 HelloWorld topic;
 deserialize_HelloWorld_topic(serialized_topic, &topic);

 configPRINTF( ("Receive topic: %s, count: %i\n", topic.message, topic.index) );
 g_subscribe_cnt++;
 if(g_loop_out == 2) {
 sprintf(g_last_rcv_buf, "Receive topic: %s, count: %i", topic.message, topic.index);
 g_loop_out = 1;
 }
 }
}



void setup(){

 pinMode(PIN_LED0, OUTPUT);
}

void loop(){
 Session my_session;
 ObjectId datareader_id;
 ClientKey key = {{0xBB, 0xBB, 0xBB, 0xBB}};

#ifdef demoConfigMICRORTPS_USE_SERIAL
 /* Device name does not need to be set.
 * Serial device used by client is dependent on the board revision. */
 if(!new_serial_session(&my_session, 0x01, key, " ", on_topic, NULL))
 {
 configPRINTF( ("Can not create serial connection\n") );
 } else {
 configPRINTF( ("<< Serial mode >>\n") );
 }
#elif defined demoConfigMICRORTPS_USE_UDP
 /* Wait for network configuration. */

 ms_sleep(10000);
 if(!new_udp_session(&my_session, 0x01, key, agentIPAddress, agentPort, on_topic, NULL))
 {
 configPRINTF( ("Can not create a socket\n") );
 } else {
 configPRINTF( ("<< UDP mode >>\n") );
 }
#endif

 /* Init session. */
 init_session_sync(&my_session);
 check_and_print_error(&my_session, "init_session");

 /* Init XRCE objects. */
 ObjectId participant_id = {{0x00, OBJK_PARTICIPANT}};
 create_participant_sync_by_ref(&my_session, participant_id, "default_participant", false, false);
 check_and_print_error(&my_session, "create participant");

 const char* topic_xml = {"<dds><topic><name>HelloWorldTopic</name><dataType>micrortps_demo_msgs::msg::dds_::HelloWorld_</dataType></topic></dds>"};
 ObjectId topic_id = {{0x00, OBJK_TOPIC}};
 create_topic_sync_by_xml(&my_session, topic_id, topic_xml, participant_id, false, false);
 check_and_print_error(&my_session, "create topic");

 const char* subscriber_xml = {"<subscriber name=\"MySubscriber\""};
 ObjectId subscriber_id = {{HELLO_WORLD_TOPIC, OBJK_SUBSCRIBER}};
 create_subscriber_sync_by_xml(&my_session, subscriber_id, subscriber_xml, participant_id, false, false);
 check_and_print_error(&my_session, "create subscriber");

 /* Set partition name of ROS2 topics, "rt" to communicate with ROS2 publisher and subscriber.
 * About ROS Specific Namespace, please check following link:
 * design.ros2.org/.../topic_and_service_names.html
 * When micro RTPS client communicate with FreeRTPS HelloWorld example, partition name should be removed. */
 const char* datareader_xml = {"<profiles><subscriber profile_name=\"default_xrce_subscriber_profile\"><topic><kind>NO_KEY</kind><name>HelloWorldTopic</name><dataType>micrortps_demo_msgs::msg::dds_::HelloWorld_</dataType><historyQos><kind>KEEP_LAST</kind><depth>5</depth></historyQos><durability><kind>TRANSIENT_LOCAL</kind></durability></topic><qos><partition><names><name>rt</name></names></partition></qos></subscriber></profiles>"};
 datareader_id = {{HELLO_WORLD_TOPIC, OBJK_DATAREADER}};
 create_datareader_sync_by_xml(&my_session, datareader_id, datareader_xml, subscriber_id, false, false);
 check_and_print_error(&my_session, "create datareader");

 /* Main loop */
 for( ; ; )
 {
 /* Request data */
 read_data_sync(&my_session, datareader_id, democonfigMICRORTPS_SELECT_STREAM);
 check_and_print_error(&my_session, "read_data");

 run_communication(&my_session);

 ms_sleep(democonfigMICRORTPS_SUB_TASK_DELAY);
 }

 close_session_sync(&my_session);
 check_and_print_error(&my_session, "close session");

 free_session(&my_session);

 digitalWrite(PIN_LED0, !digitalRead(PIN_LED0));
 vTaskDelay(500);
}
#endif /* demoConfigMICRORTPS_SELECT_SUBSCRIBER */


#ifdef demoConfigMICRORTPS_SELECT_PUBLISHER

static void prvMicrortpsPub(void * pvParameters);

#ifdef demoConfigMICRORTPS_USE_UDP
static const uint8_t agentIPAddress[4] =
{
 democonfigMICRORTPS_AGENT_ADDR0,
 democonfigMICRORTPS_AGENT_ADDR1,
 democonfigMICRORTPS_AGENT_ADDR2,
 democonfigMICRORTPS_AGENT_ADDR3
};
static const uint16_t agentPort = democonfigMICRORTPS_AGENT_PORT;
#endif

void check_and_print_error(Session* session, const char* where)
{
 if(session->last_status_received)
 {
 if(session->last_status.status == STATUS_OK)
 {
 return; //All things go well
 }
 else
 {
 configPRINTF( ("Status error (%i)", session->last_status.status) );
 }
 }
 else
 {
 configPRINTF( ("Connection error") );
 }
 configPRINTF( (" at %s\n", where) );

 /*TODO: When the connection is failed, client should close
 * session and reconnect to agent(not implemented yet). */
 return;
}

void setup(){

 pinMode(PIN_LED0, OUTPUT);
}

void loop(){
 Session my_session;
 ObjectId datawriter_id;

 ClientKey key = {{0xAA, 0xAA, 0xAA, 0xAA}};

#ifdef demoConfigMICRORTPS_USE_SERIAL
 /* Device name does not need to be set.
 * Serial device used by client is dependent on the board revision. */
 if(!new_serial_session(&my_session, 0x01, key, " ", NULL, NULL))
 {
 configPRINTF( ("Can not create serial connection\n") );
 } else {
 configPRINTF( ("<< Serial mode >>\n") );
 }
#elif defined demoConfigMICRORTPS_USE_UDP
 /* Wait for network configuration. */
 ms_sleep(15000);
#ifdef swValidate_Connect
 while(g_loop_out == 0) {
 ms_sleep(5000);
 }
#endif

 if(!new_udp_session(&my_session, 0x01, key, agentIPAddress, agentPort, NULL, NULL))
 {
 configPRINTF( ("Can not create a socket\n") );
 } else {
 configPRINTF( ("<< UDP mode >>\n") );
 }
#endif

 /* Init session. */
 init_session_sync(&my_session);
 check_and_print_error(&my_session, "init session");

 /* Init XRCE objects. */
 ObjectId participant_id = {{0x00, OBJK_PARTICIPANT}};
 create_participant_sync_by_ref(&my_session, participant_id, "default_participant", false, false);
 check_and_print_error(&my_session, "create participant");

 const char* topic_xml = {"<dds><topic><name>HelloWorldTopic</name><dataType>micrortps_demo_msgs::msg::dds_::HelloWorld_</dataType></topic></dds>"};
 ObjectId topic_id = {{0x00, OBJK_TOPIC}};
 create_topic_sync_by_xml(&my_session, topic_id, topic_xml, participant_id, false, false);
 check_and_print_error(&my_session, "create topic");

 const char* publisher_xml = {"<publisher name=\"MyPublisher\""};
 ObjectId publisher_id = {{0x00, OBJK_PUBLISHER}};
 create_publisher_sync_by_xml(&my_session, publisher_id, publisher_xml, participant_id, false, false);
 check_and_print_error(&my_session, "create publisher");

 /* Set partition name of ROS2 topics, "rt" to communicate with ROS2 publisher and subscriber.
 * About ROS Specific Namespace, please check following link:
 * design.ros2.org/.../topic_and_service_names.html
 * When micro RTPS client communicate with FreeRTPS HelloWorld example, partition name should be removed. */
 const char* datawriter_xml = {"<profiles><publisher profile_name=\"default_xrce_publisher_profile\"><topic><kind>NO_KEY</kind><name>HelloWorldTopic</name><dataType>micrortps_demo_msgs::msg::dds_::HelloWorld_</dataType><historyQos><kind>KEEP_LAST</kind><depth>5</depth></historyQos><durability><kind>TRANSIENT_LOCAL</kind></durability></topic><qos><partition><names><name>rt</name></names></partition></qos></publisher></profiles>"};
 datawriter_id = {{0x00, OBJK_DATAWRITER}};
 create_datawriter_sync_by_xml(&my_session, datawriter_id, datawriter_xml, publisher_id, false, false);
 check_and_print_error(&my_session, "create datawriter");

 /* Main loop */
 int32_t count = 0;
 for( ; ; )
 {
 /* Write HelloWorld topic */
 HelloWorld topic;
 topic.index = ++count;
 topic.message = "Hello DDS world!";
 write_HelloWorld(&my_session, datawriter_id, democonfigMICRORTPS_SELECT_STREAM, &topic);
 configPRINTF( ("Send topic: %s, count: %i\n", topic.message, topic.index) );

 run_communication(&my_session);

 ms_sleep(democonfigMICRORTPS_PUB_TASK_DELAY);
 }

 close_session_sync(&my_session);
 check_and_print_error(&my_session, "close session");

 free_session(&my_session);

 digitalWrite(PIN_LED0, !digitalRead(PIN_LED0));
 vTaskDelay(500);
}

#endif /* demoConfigMICRORTPS_SELECT_PUBLISHER */