Skip to content

Commit fa5f82d

Browse files
pablogs9pablogs9
authored
Add wifi transport (#169)
1 parent b287448 commit fa5f82d

File tree

4 files changed

+182
-15
lines changed

4 files changed

+182
-15
lines changed

‎.github/workflows/ci.yml

Lines changed: 16 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,8 @@ jobs:
2121
apt update
2222
apt install -y git curl lib32z1 wget libfontconfig libxft2 xz-utils rsync
2323
curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | sh
24-
mkdir -p /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/
25-
cp -R checkout/* /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/
24+
mkdir -p /github/home/Arduino/libraries/micro_ros_arduino/
25+
cp -R checkout/* /github/home/Arduino/libraries/micro_ros_arduino/
2626
echo '''board_manager:
2727
additional_urls:
2828
- https://raw.githubusercontent.com/ROBOTIS-GIT/OpenCR/master/arduino/opencr_release/package_opencr_index.json''' > arduino-cli.yaml
@@ -55,17 +55,18 @@ jobs:
5555
arduino-cli core update-index
5656
arduino-cli lib update-index
5757
# Build all demos
58-
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
59-
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_addtwoints_service -v
60-
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_reconnection_example -v
61-
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_subscriber -v
62-
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_subscriber_twist -v
63-
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_tf_publisher -v
58+
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
59+
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_addtwoints_service -v
60+
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_reconnection_example -v
61+
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_subscriber -v
62+
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_subscriber_twist -v
63+
arduino-cli compile --fqbn OpenCR:OpenCR:OpenCR /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_tf_publisher -v
6464
# Build one demo for each platform
65-
arduino-cli compile --fqbn teensy:avr:teensy31 /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
66-
arduino-cli compile --fqbn teensy:avr:teensy35 /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
67-
arduino-cli compile --fqbn teensy:avr:teensy41 /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
68-
arduino-cli compile --fqbn arduino:samd:arduino_zero_native /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
69-
arduino-cli compile --fqbn arduino:sam:arduino_due_x /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
70-
# arduino-cli compile --fqbn arduino:mbed:envie_m4 /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
71-
arduino-cli compile --fqbn arduino:mbed:envie_m7 /github/home/Arduino/libraries/micro_ros_arduino-0.0.1/examples/micro-ros_publisher -v
65+
arduino-cli compile --fqbn teensy:avr:teensy31 /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
66+
arduino-cli compile --fqbn teensy:avr:teensy35 /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
67+
arduino-cli compile --fqbn teensy:avr:teensy41 /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
68+
arduino-cli compile --fqbn arduino:samd:arduino_zero_native /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
69+
arduino-cli compile --fqbn arduino:sam:arduino_due_x /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
70+
# arduino-cli compile --fqbn arduino:mbed:envie_m4 /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
71+
arduino-cli compile --fqbn arduino:mbed:envie_m7 /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher -v
72+
arduino-cli compile --fqbn arduino:mbed:envie_m7 /github/home/Arduino/libraries/micro_ros_arduino/examples/micro-ros_publisher_wifi -v

‎examples/micro-ros_publisher_wifi/micro-ros_publisher_wifi.ino

Lines changed: 72 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,72 @@
1+
#include <micro_ros_arduino.h>
2+
3+
#include <stdio.h>
4+
#include <rcl/rcl.h>
5+
#include <rcl/error_handling.h>
6+
#include <rclc/rclc.h>
7+
#include <rclc/executor.h>
8+
9+
#include <std_msgs/msg/int32.h>
10+
11+
#ifndef TARGET_PORTENTA_H7_M7
12+
#error This example is only avaible for Arduino Portenta
13+
#endif
14+
15+
rcl_publisher_t publisher;
16+
std_msgs__msg__Int32 msg;
17+
rclc_support_t support;
18+
rcl_allocator_t allocator;
19+
rcl_node_t node;
20+
21+
#define LED_PIN 13
22+
23+
#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
24+
#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
25+
26+
27+
void error_loop(){
28+
while(1){
29+
digitalWrite(LED_PIN, !digitalRead(LED_PIN));
30+
delay(100);
31+
}
32+
}
33+
34+
void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
35+
{
36+
RCLC_UNUSED(last_call_time);
37+
if (timer != NULL) {
38+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
39+
msg.data++;
40+
}
41+
}
42+
43+
void setup() {
44+
set_microros_wifi_transports("WIFI SSID", "WIFI PASS", "192.168.1.57", 8888);
45+
46+
pinMode(LED_PIN, OUTPUT);
47+
digitalWrite(LED_PIN, HIGH);
48+
49+
delay(2000);
50+
51+
allocator = rcl_get_default_allocator();
52+
53+
//create init_options
54+
RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
55+
56+
// create node
57+
RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_wifi_node", "", &support));
58+
59+
// create publisher
60+
RCCHECK(rclc_publisher_init_best_effort(
61+
&publisher,
62+
&node,
63+
ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
64+
"topic_name"));
65+
66+
msg.data = 0;
67+
}
68+
69+
void loop() {
70+
RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
71+
msg.data++;
72+
}

‎src/micro_ros_arduino.h

Lines changed: 37 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,4 +32,41 @@ static inline void set_microros_transports(){
3232
);
3333
}
3434

35+
#ifdef TARGET_PORTENTA_H7_M7
36+
37+
#include <WiFi.h>
38+
#include <WiFiUdp.h>
39+
40+
extern "C" bool arduino_wifi_transport_open(struct uxrCustomTransport * transport);
41+
extern "C" bool arduino_wifi_transport_close(struct uxrCustomTransport * transport);
42+
extern "C" size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
43+
extern "C" size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
44+
45+
struct micro_ros_agent_locator {
46+
IPAddress address;
47+
int port;
48+
};
49+
50+
static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){
51+
while (WiFi.begin(ssid, pass) != WL_CONNECTED)
52+
{
53+
delay(500);
54+
}
55+
56+
static struct micro_ros_agent_locator locator;
57+
locator.address.fromString(agent_ip);
58+
locator.port = agent_port;
59+
60+
rmw_uros_set_custom_transport(
61+
false,
62+
(void *) &locator,
63+
arduino_wifi_transport_open,
64+
arduino_wifi_transport_close,
65+
arduino_wifi_transport_write,
66+
arduino_wifi_transport_read
67+
);
68+
}
69+
70+
#endif // TARGET_PORTENTA_H7_M7
71+
3572
#endif // MICRO_ROS_ARDUINO

‎src/wifi_transport.cpp

Lines changed: 57 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
#ifdef TARGET_PORTENTA_H7_M7
2+
3+
#include <Arduino.h>
4+
#include <WiFi.h>
5+
#include <WiFiUdp.h>
6+
7+
#include <micro_ros_arduino.h>
8+
9+
extern "C"
10+
{
11+
12+
static WiFiUDP udp_client;
13+
14+
bool arduino_wifi_transport_open(struct uxrCustomTransport * transport)
15+
{
16+
struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args;
17+
udp_client.begin(locator->port);
18+
return true;
19+
}
20+
21+
bool arduino_wifi_transport_close(struct uxrCustomTransport * transport)
22+
{
23+
udp_client.stop();
24+
return true;
25+
}
26+
27+
size_t arduino_wifi_transport_write(struct uxrCustomTransport * transport, const uint8_t *buf, size_t len, uint8_t *errcode)
28+
{
29+
(void)errcode;
30+
struct micro_ros_agent_locator * locator = (struct micro_ros_agent_locator *) transport->args;
31+
32+
udp_client.beginPacket(locator->address, locator->port);
33+
size_t sent = udp_client.write(buf, len);
34+
udp_client.endPacket();
35+
udp_client.flush();
36+
37+
return sent;
38+
}
39+
40+
size_t arduino_wifi_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode)
41+
{
42+
(void) errcode;
43+
44+
uint32_t start_time = millis();
45+
46+
while(millis() - start_time < timeout && udp_client.parsePacket() == 0){
47+
delay(1);
48+
}
49+
50+
size_t readed = udp_client.read(buf, len);
51+
52+
return (readed < 0) ? 0 : readed;
53+
}
54+
}
55+
56+
#endif // TARGET_PORTENTA_H7_M7
57+

0 commit comments

Comments
 (0)