Setup repository to separate seahawk code.

Author: mike-matera

.gitignore

@@ -0,0 +1 @@
+build

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "src/pico-sdk"]
+	path = src/pico-sdk
+	url = https://github.com/raspberrypi/pico-sdk.git
+[submodule "src/micro_ros_raspberrypi_pico_sdk"]
+	path = src/micro_ros_raspberrypi_pico_sdk
+	url = https://github.com/micro-ROS/micro_ros_raspberrypi_pico_sdk.git

.vscode/settings.json

@@ -0,0 +1,8 @@
+{
+    "cmake.configureEnvironment": {
+        "PICO_SDK_PATH": "${workspaceFolder}/src/pico-sdk",
+        "MICRO_ROS_RPI_PICO_PATH": "${workspaceFolder}/src/micro_ros_raspberrypi_pico_sdk",
+    },
+    "git.detectSubmodules": false,
+    "cmake.sourceDirectory": "${workspaceFolder}/src/seahawk",
+}

src/micro_ros_raspberrypi_pico_sdk

@@ -0,0 +1,37 @@
+cmake_minimum_required(VERSION 3.12)
+
+include($ENV{PICO_SDK_PATH}/external/pico_sdk_import.cmake)
+
+project(seahawk C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+pico_sdk_init()
+
+link_directories($ENV{MICRO_ROS_RPI_PICO_PATH}/libmicroros)
+
+add_executable(seahawk
+    seahawk.c
+    pico_uart_transport.c
+)
+target_link_libraries(seahawk
+    pico_stdlib
+    microros
+)
+
+target_include_directories(seahawk PUBLIC
+    $ENV{MICRO_ROS_RPI_PICO_PATH}/libmicroros/include
+)
+
+SET(CMAKE_C_FLAGS  "${CMAKE_C_FLAGS} -ffunction-sections -fdata-sections")
+SET(CMAKE_CXX_FLAGS  "${CMAKE_CXX_FLAGS} -ffunction-sections -fdata-sections")
+
+# Configure Pico
+pico_enable_stdio_usb(seahawk 1)
+pico_enable_stdio_uart(seahawk 0)
+add_compile_definitions(PICO_UART_ENABLE_CRLF_SUPPORT=0)
+add_compile_definitions(PICO_STDIO_ENABLE_CRLF_SUPPORT=0)
+add_compile_definitions(PICO_STDIO_DEFAULT_CRLF=0)
+
+# Generate UF2
+pico_add_extra_outputs(seahawk)

src/pico-sdk

@@ -0,0 +1,64 @@
+#include <stdio.h>
+#include "pico/stdlib.h"
+
+#include <uxr/client/profile/transport/custom/custom_transport.h>
+
+void usleep(uint64_t us)
+{
+    sleep_us(us);
+}
+
+int clock_gettime(clockid_t unused, struct timespec *tp)
+{
+    uint64_t m = time_us_64();
+    tp->tv_sec = m / 1000000;
+    tp->tv_nsec = (m % 1000000) * 1000;
+    return 0;
+}
+
+bool pico_serial_transport_open(struct uxrCustomTransport * transport)
+{
+    stdio_init_all();
+    return true;
+}
+
+bool pico_serial_transport_close(struct uxrCustomTransport * transport)
+{
+    return true;
+}
+
+size_t pico_serial_transport_write(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, uint8_t *errcode)
+{
+    for (size_t i = 0; i < len; i++)
+    {
+        if (buf[i] != putchar(buf[i]))
+        {
+            *errcode = 1;
+            return i;
+        }
+    }
+    return len;
+}
+
+size_t pico_serial_transport_read(struct uxrCustomTransport * transport, uint8_t *buf, size_t len, int timeout, uint8_t *errcode)
+{
+    uint64_t start_time_us = time_us_64();
+    for (size_t i = 0; i < len; i++)
+    {
+        int64_t elapsed_time_us = timeout * 1000 - (time_us_64() - start_time_us);
+        if (elapsed_time_us < 0)
+        {
+            *errcode = 1;
+            return i;
+        }
+
+        int character = getchar_timeout_us(elapsed_time_us);
+        if (character == PICO_ERROR_TIMEOUT)
+        {
+            *errcode = 1;
+            return i;
+        }
+        buf[i] = character;
+    }
+    return len;
+}

src/seahawk/CMakeLists.txt

@@ -0,0 +1,14 @@
+#ifndef MICRO_ROS_PICOSDK
+#define MICRO_ROS_PICOSDK
+
+#include <stdio.h>
+#include <stdint.h>
+
+#include <uxr/client/profile/transport/custom/custom_transport.h>
+
+bool pico_serial_transport_open(struct uxrCustomTransport * transport);
+bool pico_serial_transport_close(struct uxrCustomTransport * transport);
+size_t pico_serial_transport_write(struct uxrCustomTransport* transport, const uint8_t * buf, size_t len, uint8_t * err);
+size_t pico_serial_transport_read(struct uxrCustomTransport* transport, uint8_t* buf, size_t len, int timeout, uint8_t* err);
+
+#endif //MICRO_ROS_PICOSDK

src/seahawk/pico_uart_transport.c

@@ -0,0 +1,84 @@
+#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>
+#include <rmw_microros/rmw_microros.h>
+
+#include "pico/stdlib.h"
+#include "pico_uart_transports.h"
+
+const uint LED_PIN = 25;
+
+rcl_publisher_t publisher;
+std_msgs__msg__Int32 msg;
+
+void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
+{
+    rcl_ret_t ret = rcl_publish(&publisher, &msg, NULL);
+    msg.data++;
+}
+
+int main()
+{
+    rmw_uros_set_custom_transport(
+		true,
+		NULL,
+		pico_serial_transport_open,
+		pico_serial_transport_close,
+		pico_serial_transport_write,
+		pico_serial_transport_read
+	);
+
+    gpio_init(LED_PIN);
+    gpio_set_dir(LED_PIN, GPIO_OUT);
+
+    rcl_timer_t timer;
+    rcl_node_t node;
+    rcl_allocator_t allocator;
+    rclc_support_t support;
+    rclc_executor_t executor;
+
+    allocator = rcl_get_default_allocator();
+
+    // Wait for agent successful ping for 2 minutes.
+    const int timeout_ms = 1000; 
+    const uint8_t attempts = 120;
+
+    rcl_ret_t ret = rmw_uros_ping_agent(timeout_ms, attempts);
+
+    if (ret != RCL_RET_OK)
+    {
+        // Unreachable agent, exiting program.
+        return ret;
+    }
+
+    rclc_support_init(&support, 0, NULL, &allocator);
+
+    rclc_node_init_default(&node, "pico_node", "", &support);
+    rclc_publisher_init_default(
+        &publisher,
+        &node,
+        ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
+        "pico_publisher");
+
+    rclc_timer_init_default(
+        &timer,
+        &support,
+        RCL_MS_TO_NS(1000),
+        timer_callback);
+
+    rclc_executor_init(&executor, &support.context, 1, &allocator);
+    rclc_executor_add_timer(&executor, &timer);
+
+    gpio_put(LED_PIN, 1);
+
+    msg.data = 0;
+    while (true)
+    {
+        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
+    }
+    return 0;
+}