GR-SAKURA
GR-KURUMI
GR-COTTON
GR-CITRUS
GR-PEACH
GR-KAEDE
GR-ADZUKI
GR-LYCHEE
GR-ROSE
GR-MANGO(*)
SNShield
Web Compiler
IDE for GR
TOPPERS関連
女子美コラボ
その他
※プロデューサミーティング中
作り方使い方資料
イベント関連
作品記事
体験記事
ライブラリ
ツール
その他・過去ファイル
2年前のこのスレと同じ状況ですが、状況かなり変わってるので新しいスレを立ち上げました。
https://japan.renesasrulz.com/gr_user_forum_japanese/f/gr-rose/5856/dds-xrce
ROS2バージョン:
Foxy Fitzroy
Micro-XRCE-DDS-Agentバージョン:
1.1.6
スケッチ例:
udp_listener_besteffort (スケッチに対する変更があるので、最後に全スースコードを貼り付けます。)
現象:
コマンド「./MicroXRCEAgent udp -p 2018 -d」を実行しても、GR-ROSE側は「Discovery Agent...」のままです。
聞きたいこと:
1.byte mac[] = { 0x74, 0x90, 0x50, 0x00, 0x79, 0x03 }の部分でそのまま変更しなくてもいいですか?このMACアドレスはどの機器のことを指していますか?
2.IPAddress ipの部分も決め打ちで入力されていますが、どのIPアドレスを指していますか?変更する必要はありますか?(ソースコードではWIFIのIPアドレスを入力しました)
3.ROS側で「./MicroXRCEAgent udp -p 2018 -d」を実行すると、
[1612073847.646286] info | UDPServerLinux.cpp | init | running... | port: 2018[1612073847.647045] info | DiscoveryServerLinux.cpp | init | running... | Port: 7400
の2行が表示されますが、ここのport2018と7400は何を意味していますか?2018をポート番号にする理由を教えてください。
4.どうすればうまく通信できますか?
ソースコード:
#include <Arduino.h>#include <WiFiEsp.h>#include <Ethernet.h>#include <stdio.h>#include <string.h>#include <stdlib.h>#include <limits.h>#include <uxr/client/client.h>#include <ucdr/microcdr.h>extern "C" {#include <FreeRTOS.h>#include <task.h>}#include <JointState.h>#include <ICS.h>#define KRS_MIN 3500#define KRS_MAX 11500#define KRS_ORG 7500#define KRS_FREE 0#define SERVO_MIN -135#define SERVO_MAX 135IcsController ICS(Serial1);IcsServo roll;IcsServo pitch;IcsServo yaw;// 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 0xCCCCDDD1#define BUFFER_SIZE UXR_CONFIG_UDP_TRANSPORT_MTUchar ssid[] = "SPWN_H37_6B40DA"; // your network SSID (name)char pass[] = "********"; // your network passwordint status = WL_IDLE_STATUS; // the Wifi radio's statusbyte mac[] = { 0x74, 0x90, 0x50, 0x00, 0x79, 0x03 };IPAddress ip(192, 168, 99, 1);uxrSession session;uxrUDPTransport transport;uxrUDPPlatform udp_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 = false;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); // Initialize Servos ICS.begin(); yaw.attach(ICS, 1); pitch.attach(ICS, 2); roll.attach(ICS, 3); yaw.setPosition(KRS_ORG); delay(1); pitch.setPosition(KRS_ORG); delay(1); roll.setPosition(KRS_ORG); delay(1); // Initialize Network 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 udp_platform.use_ethernet = use_ethernet; if (!uxr_init_udp_transport(&transport, &udp_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/joint_states</name>" "<dataType>sensor_msgs::msg::dds_::JointState_</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/joint_states</name>" "<dataType>sensor_msgs::msg::dds_::JointState_</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; JointState topic; JointState_deserialize_topic(mb, &topic); Serial.println("Received topic: "); for(int i = 0; i < topic.position_size; i++){ Serial.print((char*)topic.name[i]); Serial.print("\t"); Serial.print(topic.position[i]); Serial.println(); } float radian_roll = topic.position[0]; float radian_pitch = topic.position[1]; float radian_yaw = topic.position[2]; int axis_roll = radian_roll * 180 / PI; int axis_pitch = radian_pitch * 180 / PI; int axis_yaw = radian_yaw * 180 / PI; int pos_roll = map(axis_roll, SERVO_MIN, SERVO_MAX, KRS_MIN, KRS_MAX); int pos_pitch = map(axis_pitch, SERVO_MIN, SERVO_MAX, KRS_MIN, KRS_MAX); int pos_yaw = map(axis_yaw, SERVO_MIN, SERVO_MAX, KRS_MIN, KRS_MAX); if(pos_roll > KRS_MIN && pos_roll < KRS_MAX){ roll.setPosition(pos_roll); delay(1); } if(pos_pitch > KRS_MIN && pos_pitch < KRS_MAX){ pitch.setPosition(pos_pitch); delay(1); } if(pos_yaw > KRS_MIN && pos_yaw < KRS_MAX){ yaw.setPosition(pos_yaw); delay(1); } // 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_udp_transport(&transport); // Delete this task vTaskDelete(NULL);}
------------------------------------------------------------------------------------------
長くなりましたが、よろしくお願いします。
早速のご返答ありがとうございます。質問1と2が理解できました。質問3のポート番号を自由に変更してもいいですか?luteciaさんの記事では2020になっていますが、GR-ROSE側からそれに合わせるための何かの設定が要りますか?質問4について、Ubuntuは(window10の)docker desktop上のコンテナー上で動いていて、GR-ROSEは同じWi-Fiに繋がっています。これから試すこと:luteciaさんの記事と同じくMicro-XRCE-DDS-Agentのv1.1.6を使っています、ROS2とGR-ROSEの通信に集中してソースコードを最小限にしてテストします。それでもだめなら同じ手順でDashingでもう一回試してみます。また報告させて頂きます。
3のポート番号についてですが、GR-ROSE側は設定は不要です。
uxr_discovery_agents_default(INT_MAX, 1000, on_agent_found, &chosen);
上記の部分でDisoveryを始めますが、Agentからのブロードキャスト通信を受け取ったとき、chosenにポート番号が自動で入ります。もともと2018とか2020という数字は以下のeProsimaの例文から取ってきただけです(もしかしたら暗黙的に数字が決まっているのかもしれませんが)
https://micro-xrce-dds.docs.eprosima.com/en/latest/quickstart.html
4について、Dockerで試したことはないのですが、Agentを起動したときにブロードキャスト通信が実際にされるかがよく分からないところですね。Windowsが間に入ったときに何かしらブロックしているかが気になります。
ros2 Dashing + Micro-XRCE-DDS-Agent v1.1.6の組み合わせでも通信はうまくいきませんでした。やっぱり原因は他にあるかもしれないです。AgentをDocker上で動かすには他の設定がいるかどうかをまた調べます。