複数のGR-ROSEをROS2に接続する方法

初めて質問させていただきます。

GR-ROSE1台をMicro-XREC-DDS-Agentを利用してUbuntuに接続し、通信できますが、

GR-ROSE複数台(2, 3台)をROS2に接続する方法が分かりません。

無線で接続することもありますが基本的に有線接続しています。

また、Micro-XRCE-DDS-Agentのプログラムは何も変更していないです。

どのようにすればよいか教えていただけると幸いです。

宜しくお願いします。

下記に私が使用している環境・プログラム(GR-ROSE側・ROS2側)・GR-ROSEを一台だけ接続するときのUbuntuのターミナルとGR-ROSEのシリアルモニターの状況を載せます。

OS: Ubuntu 18.04.5 LTS

ROS2: Dashing Diademata

Micro-XRCE-DDS-Agent: v1.1.0

GR-ROSE側のプログラム (tcp_listener_besteffort.ino もともとIDE4GRなどで用意されているプログラムです。IPAddressだけ変更しています。)

// Copyright 2018 eSOL Co.,Ltd.
// Copyright 2018 Proyectos y Sistemas de Mantenimiento SL (eProsima).
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// www.apache.org/.../LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <Arduino.h>
#include <WiFiEsp.h>
#include <Ethernet.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <limits.h>

#include "ros2_msg/Ros2String.h"

#include <uxr/client/client.h>
#include <ucdr/microcdr.h>

extern "C" {
#include <FreeRTOS.h>
#include <task.h>
}

// Micro-XRCE-DDS-Client key identifier of the client.
// If multiple clients connect to the same agent, each client must have a different key.
#define CLIENT_KEY 0xCCCCDDDD

#define BUFFER_SIZE UXR_CONFIG_TCP_TRANSPORT_MTU

char ssid[] = "SSID"; // your network SSID (name)
char pass[] = "PASSWORD"; // your network password
int status = WL_IDLE_STATUS; // the Wifi radio's status
byte mac[] = { 0x74, 0x90, 0x50, 0x00, 0x79, 0x03 };
IPAddress ip(192, 168, 0, 3);

uxrSession session;
uxrTCPTransport transport;
uxrTCPPlatform tcp_platform;
uxrStreamId output_stream;
uxrStreamId input_stream;
static char agent_ip[32] = {0};

uint8_t output_best_effort_stream_buffer[BUFFER_SIZE];
uint8_t input_best_effort_stream_buffer[BUFFER_SIZE];

static bool use_ethernet = true;

void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* mb, void* args);
void on_agent_found(const uxrAgentAddress* address, void* args);
static void prvUXRManagerTask(void * pvParameters);

void setup() {
// Initialize the LED pin
pinMode(PIN_LED1, OUTPUT);

if(!use_ethernet)
{
// initialize serial for debugging
Serial.begin(115200);
// initialize serial for ESP module
Serial6.begin(115200);
// initialize ESP module
WiFi.init(&Serial6);
}
else
{
// Serial output to USB
Serial.begin(9600);
}

if(!use_ethernet)
{
// check for the presence of the shield
if (WiFi.status() == WL_NO_SHIELD) {
Serial.println("WiFi shield not present");
// don't continue
while (true);
}
// attempt to connect to WiFi network
while ( status != WL_CONNECTED) {
Serial.print("Attempting to connect to WPA SSID: ");
Serial.println(ssid);
// Connect to WPA/WPA2 network
status = WiFi.begin(ssid, pass);
}
}
else
{
// Setting IP
Ethernet.begin(mac, ip);
}

// Wait for network configuration
vTaskDelay(5000);

// Discovery Agent
Serial.println("Discovery Agent...");
uxrAgentAddress chosen;
chosen.ip = agent_ip;

// Choose Ethernet or WiFi
uxr_discovery_choose_ethernet(use_ethernet);

// Try forever until Agent is found
uxr_discovery_agents_default(INT_MAX, 1000, on_agent_found, &chosen);

Serial.print("Chosen agent => ip: ");
Serial.print(chosen.ip);
Serial.print(", port: ");
Serial.println(chosen.port);

// Transport
tcp_platform.use_ethernet = use_ethernet;
if (!uxr_init_tcp_transport(&transport, &tcp_platform, chosen.ip, chosen.port))
{
Serial.println("Error at create transport.");
return;
}

// Session
uxr_init_session(&session, &transport.comm, CLIENT_KEY);
uxr_set_topic_callback(&session, on_topic, NULL);
if (!uxr_create_session(&session))
{
Serial.println("Error at create session.");
return;
}

// Streams
output_stream = uxr_create_output_best_effort_stream(&session, output_best_effort_stream_buffer, BUFFER_SIZE);
input_stream = uxr_create_input_best_effort_stream(&session);

// Create entities
uxrObjectId participant_id = uxr_object_id(0x01, UXR_PARTICIPANT_ID);
const char* participant_xml = "<dds>"
"<participant>"
"<rtps>"
"<name>default_xrce_participant</name>"
"</rtps>"
"</participant>"
"</dds>";
uint16_t participant_req = uxr_buffer_create_participant_xml(&session, output_stream, participant_id, 0, participant_xml, UXR_REPLACE);

// In order to avoid buffer overflow, uxr_flash_output_streams() has to be
// called everytime entities message is created.
(void) uxr_flash_output_streams(&session);

uxrObjectId topic_id = uxr_object_id(0x01, UXR_TOPIC_ID);
const char* topic_xml = "<dds>"
"<topic>"
"<name>rt/chatter</name>"
"<dataType>std_msgs::msg::dds_::String_</dataType>"
"</topic>"
"</dds>";
uint16_t topic_req = uxr_buffer_create_topic_xml(&session, output_stream, topic_id, participant_id, topic_xml, UXR_REPLACE);

// In order to avoid buffer overflow, uxr_flash_output_streams() has to be
// called everytime entities message is created.
(void) uxr_flash_output_streams(&session);

uxrObjectId subscriber_id = uxr_object_id(0x01, UXR_SUBSCRIBER_ID);
const char* subscriber_xml = "";
uint16_t subscriber_req = uxr_buffer_create_subscriber_xml(&session, output_stream, subscriber_id, participant_id, subscriber_xml, UXR_REPLACE);

// In order to avoid buffer overflow, uxr_flash_output_streams() has to be
// called everytime entities message is created.
(void) uxr_flash_output_streams(&session);

uxrObjectId datareader_id = uxr_object_id(0x01, UXR_DATAREADER_ID);
const char* datareader_xml = "<dds>"
"<data_reader>"
"<topic>"
"<kind>NO_KEY</kind>"
"<name>rt/chatter</name>"
"<dataType>std_msgs::msg::dds_::String_</dataType>"
"</topic>"
"</data_reader>"
"</dds>";
uint16_t datareader_req = uxr_buffer_create_datareader_xml(&session, output_stream, datareader_id, subscriber_id, datareader_xml, UXR_REPLACE);

// In order to avoid buffer overflow, uxr_flash_output_streams() has to be
// called everytime entities message is created.
(void) uxr_flash_output_streams(&session);

// Request topics
uxrDeliveryControl delivery_control = {0};
delivery_control.max_samples = UXR_MAX_SAMPLES_UNLIMITED;
(void) uxr_buffer_request_data(&session, output_stream, datareader_id, input_stream, &delivery_control);

xTaskCreate(prvUXRManagerTask, "ListenerDemo", configMINIMAL_STACK_SIZE * 5, NULL, 2, NULL);
}

void loop() {
// Do nothing
vTaskDelay(10000);
}

void on_topic(uxrSession* session, uxrObjectId object_id, uint16_t request_id, uxrStreamId stream_id, struct ucdrBuffer* mb, void* args) {
(void) session; (void) object_id; (void) request_id; (void) stream_id;

Ros2String topic;
Ros2String_deserialize_topic(mb, &topic);

Serial.print("Received topic: ");
Serial.println(topic.data);

// Toggle the heartbeat LED
digitalWrite(PIN_LED1, !digitalRead(PIN_LED1));
}

void on_agent_found(const uxrAgentAddress* address, void* args) {
uxrAgentAddress* agent = (uxrAgentAddress*)args;

Serial.print("Found agent => ip: ");
Serial.print(address->ip);
Serial.print(", port: ");
Serial.println(address->port);

memcpy((void*)(agent->ip), address->ip, 32);
agent->port = address->port;
}

static void prvUXRManagerTask(void * pvParameters) {
// Cannot confirm whether client is connected to agent or not.
while (1)
{
// Receive topics
(void) uxr_run_session_time(&session, 1);

vTaskDelay(1);
}

// Delete resources
(void) uxr_delete_session(&session);
(void) uxr_close_tcp_transport(&transport);

// Delete this task
vTaskDelete(NULL);
}

Ubuntu側のプログラム(ROS2)

// Copyright 2014 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// www.apache.org/.../LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <chrono>
#include <cstdio>
#include <memory>
#include <string>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>

using namespace std::chrono_literals;

class Talker : public rclcpp::Node
{
public:
explicit Talker(const std::string & topic_name)
: Node("talker"), count_(0)
{
// タイマー実行されるイベントハンドラー関数
auto publish_message =
[this]() -> void // ラムダ式による関数オブジェクトの定義
{
// 送信するメッセージ
auto msg = std::make_unique<std_msgs::msg::String>();
msg->data = "Hello world!" + std::to_string(count_++);

RCLCPP_INFO(this->get_logger(), "%s", msg->data.c_str());
pub_->publish(std::move(msg));
};

// chatterトピックの送信設定
rclcpp::QoS qos(rclcpp::KeepLast(10));
pub_ = create_publisher<std_msgs::msg::String>(topic_name, qos);
// publish_messageの1000ミリ秒周期でのタイマー実行
timer_ = create_wall_timer(1000ms, publish_message);
}

private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
rclcpp::TimerBase::SharedPtr timer_;
size_t count_;
};

int main(int argc, char * argv[])
{
// クライアントライブラリの初期化
setvbuf(stdout, NULL, _IONBF, BUFSIZ);
rclcpp::init(argc, argv);

// talkerノードの生成とスピン開始
auto node = std::make_shared<Talker>("chatter");
rclcpp::spin(node);
rclcpp::shutdown();

return 0;
}

Ubuntuのターミナル1(Micro-XRCE-DDS-Agent)

bash: install/local_setup.bash: そのようなファイルやディレクトリはありません
b0toku@b0toku-System-Product-Name:~$ cd micro-XRCE-DDS-agent
b0toku@b0toku-System-Product-Name:~/micro-XRCE-DDS-agent$ cd build
b0toku@b0toku-System-Product-Name:~/micro-XRCE-DDS-agent/build$ LD_PRELOAD="/usr/local/lib/libfastrtps.so.1 /usr/local/lib/libfastcdr.so.1" MicroXRCEAgent tcp -p 2020 -d
Enter 'q' for exit
[1608177907.777215] info | TCPServerLinux.cpp | init | running... | port: 2020
[1608177907.777541] info | DiscoveryServerLinux.cpp | init | running... | Port: 7400
[1608177907.919927] info | Root.cpp | create_client | create | client_key: 0xCCCCDDDD, session_id: 0x81
[1608177907.919991] info | TCPServerBase.cpp | on_create_client | session established | client_key: 0xCCCCDDDD, address: 192.168.0.3:13549

Ubuntuのターミナル2(talkerのプログラム)

bash: install/local_setup.bash: そのようなファイルやディレクトリはありません
b0toku@b0toku-System-Product-Name:~$ cd micro-XRCE-DDS-agent
b0toku@b0toku-System-Product-Name:~/micro-XRCE-DDS-agent$ cd build
b0toku@b0toku-System-Product-Name:~/micro-XRCE-DDS-agent/build$ LD_PRELOAD="/usr/local/lib/libfastrtps.so.1 /usr/local/lib/libfastcdr.so.1" MicroXRCEAgent tcp -p 2020 -d
Enter 'q' for exit
[1608177907.777215] info | TCPServerLinux.cpp | init | running... | port: 2020
[1608177907.777541] info | DiscoveryServerLinux.cpp | init | running... | Port: 7400
[1608177907.919927] info | Root.cpp | create_client | create | client_key: 0xCCCCDDDD, session_id: 0x81
[1608177907.919991] info | TCPServerBase.cpp | on_create_client | session established | client_key: 0xCCCCDDDD, address: 192.168.0.3:13549

GR-ROSEのシリアルモニター