portenta h7 m7 ethernet support (#852) (#863)

* add ethernet trasport for PORTENTA_H7_M7 in uros header * add ethernet trasport for PORTENTA_H7_M7 cpp * add ethernet publish example * correct error msg add * add comment ip * change topic_name, namespace * update mac address Co-authored-by: Pablo Garrido <[email protected]> * remove comment Co-authored-by: Pablo Garrido <[email protected]> * remove not used headers * fix error condition Co-authored-by: Pablo Garrido <[email protected]> (cherry picked from commit b12ed9b) Co-authored-by: Jenifen <[email protected]>
micro-ROS · Mar 21, 2022 · 9d48bd8 · 9d48bd8
1 parent 26a3931
commit 9d48bd8
Show file tree

Hide file tree

Showing 3 changed files with 89 additions and 3 deletions.
diff --git a/examples/micro-ros_publisher_ethernet/micro-ros_publisher_ethernet.ino b/examples/micro-ros_publisher_ethernet/micro-ros_publisher_ethernet.ino
@@ -0,0 +1,76 @@
+#include <micro_ros_arduino.h>
+
+#include <stdio.h>
+#include <rcl/rcl.h>
+#include <rcl/error_handling.h>
+#include <rclc/rclc.h>
+#include <rclc/executor.h>
+#include <std_msgs/msg/int32.h>
+
+#if !defined(TARGET_STM32F4) && !defined(ARDUINO_TEENSY41) && !defined(TARGET_PORTENTA_H7_M7)
+#error This example is only avaible for Arduino Portenta, Arduino Teensy41 and STM32F4
+#endif
+
+rcl_publisher_t publisher;
+std_msgs__msg__Int32 msg;
+rclc_support_t support;
+rcl_allocator_t allocator;
+rcl_node_t node;
+
+#define LED_PIN 13
+
+#define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){error_loop();}}
+#define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){}}
+
+
+
+void error_loop(){
+  while(1){
+    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
+    delay(100);
+  }
+}
+
+void timer_callback(rcl_timer_t * timer, int64_t last_call_time)
+{
+  RCLC_UNUSED(last_call_time);
+  if (timer != NULL) {
+    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
+    msg.data++;
+  }
+}
+
+void setup() {
+  byte arduino_mac[] = { 0xAA, 0xBB, 0xCC, 0xEE, 0xDD, 0xFF };
+  IPAddress arduino_ip(192, 168, 1, 177);
+  IPAddress agent_ip(192, 168, 1, 113);
+  set_microros_native_ethernet_udp_transports(arduino_mac, arduino_ip, agent_ip, 9999);
+
+  pinMode(LED_PIN, OUTPUT);
+  digitalWrite(LED_PIN, HIGH);
+
+  delay(2000);
+
+  allocator = rcl_get_default_allocator();
+
+  //create init_options
+  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));
+
+  // create node
+  RCCHECK(rclc_node_init_default(&node, "micro_ros_arduino_ethernet_node", "namespace", &support));
+
+  // create publisher
+  RCCHECK(rclc_publisher_init_best_effort(
+    &publisher,
+    &node,
+    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+    "topic_name"));
+
+  msg.data = 0;
+}
+
+void loop() {
+    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
+    msg.data++;
+    delay(100);
+}
diff --git a/src/micro_ros_arduino.h b/src/micro_ros_arduino.h
@@ -48,7 +48,11 @@ static inline void set_microros_transports(){
 #include <NativeEthernet.h>
 #endif
 
-#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41)
+#if defined(TARGET_PORTENTA_H7_M7) 
+#include <PortentaEthernet.h>
+#endif
+
+#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41)  || defined(TARGET_PORTENTA_H7_M7)
 extern "C" bool arduino_native_ethernet_udp_transport_open(struct uxrCustomTransport * transport);
 extern "C" bool arduino_native_ethernet_udp_transport_close(struct uxrCustomTransport * transport);
 extern "C" size_t arduino_native_ethernet_udp_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
@@ -95,11 +99,12 @@ extern "C" bool arduino_wifi_transport_open(struct uxrCustomTransport * transpor
 extern "C" bool arduino_wifi_transport_close(struct uxrCustomTransport * transport);
 extern "C" size_t arduino_wifi_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
 extern "C" size_t arduino_wifi_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
-
+#ifndef TARGET_PORTENTA_H7_M7
 struct micro_ros_agent_locator {
 	IPAddress address;
 	int port;
 };
+#endif
 
 static inline void set_microros_wifi_transports(char * ssid, char * pass, char * agent_ip, uint agent_port){
 

diff --git a/src/native_ethernet_transport.cpp b/src/native_ethernet_transport.cpp
@@ -10,8 +10,13 @@
 #include <NativeEthernet.h>
 #include <micro_ros_arduino.h>
 #endif
+#ifdef TARGET_PORTENTA_H7_M7
+#include <Arduino.h>
+#include <EthernetUdp.h>
+#include <micro_ros_arduino.h>
+#endif
 
-#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41)
+#if defined(TARGET_STM32F4) || defined(ARDUINO_TEENSY41) || defined(TARGET_PORTENTA_H7_M7)
 extern "C" {
 
 #include <stdbool.h>