diff --git a/.github/workflows/firmware_build.yml b/.github/workflows/firmware_build.yml index 7a29b2a..b4d5591 100644 --- a/.github/workflows/firmware_build.yml +++ b/.github/workflows/firmware_build.yml @@ -5,13 +5,20 @@ on: jobs: firmware-build: name: Firmware build - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 steps: - name: Checkout uses: actions/checkout@v2 + with: + submodules: true + # - name: Installing platformio + # run: pip3 install -U platformio - - name: Installing platformio - run: pip3 install -U platformio + # - name: Building a firmware for STM32 + # run: git submodule update --init --recursive && pio run - name: Building a firmware for STM32 - run: git submodule update --init --recursive && pio run + run: | + set -e + docker compose build --no-cache + docker compose up --exit-code-from pio \ No newline at end of file diff --git a/.github/workflows/firmware_release.yml b/.github/workflows/firmware_release.yml index 1189780..eb719e2 100644 --- a/.github/workflows/firmware_release.yml +++ b/.github/workflows/firmware_release.yml @@ -49,16 +49,24 @@ jobs: firmware-release: name: Firmware release needs: bump-version - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 steps: - name: Checkout - uses: actions/checkout@v2 + uses: actions/checkout@v4 + with: + submodules: true + + # - name: Installing platformio + # run: pip3 install -U platformio - - name: Installing platformio - run: pip3 install -U platformio + # - name: Building a firmware for STM32 + # run: git submodule update --init --recursive && pio run - name: Building a firmware for STM32 - run: git submodule update --init --recursive && pio run + run: | + set -e + docker compose build --no-cache + docker compose up --exit-code-from pio - name: Release a firmware uses: softprops/action-gh-release@v1 diff --git a/.mbedignore b/.mbedignore index 81201a3..44e3e3c 100644 --- a/.mbedignore +++ b/.mbedignore @@ -1,9 +1,9 @@ -cellular/* -cryptocell/* -deprecated_warnings/* -lorawan/* -lwipstack/* -nanostack/* -netsocket/* -nfc/* -unsupported/* \ No newline at end of file +**/cellular/* +**/cryptocell/* +**/deprecated_warnings/* +**/lorawan/* +**/lwipstack/* +**/nanostack/* +**/netsocket/* +**/nfc/* +**/unsupported/* \ No newline at end of file diff --git a/Dockerfile b/Dockerfile new file mode 100644 index 0000000..49846d7 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,30 @@ +FROM ubuntu:22.04 + +RUN apt update && apt install -y \ + python3 \ + python3-pip \ + python3.10-venv \ + git \ + cmake + +RUN pip3 install --upgrade pip setuptools + +RUN pip3 install -U platformio + +WORKDIR /app + +COPY . . + +ENV USER=root + +RUN git config --global --add safe.directory /app && \ + git config --global --add safe.directory /app/lib/encoder-mbed && \ + git config --global --add safe.directory /app/lib/imu-driver && \ + git config --global --add safe.directory /app/lib/motor-driver-mbed && \ + git config --global --add safe.directory /app/lib/vl53l0x-mbed && \ + git submodule update --init --recursive && \ + pio lib install + +RUN python3 -m venv /root/.platformio/penv/bin/activate + +CMD pio run \ No newline at end of file diff --git a/colcon.meta b/colcon.meta index be9cf16..9a00803 100644 --- a/colcon.meta +++ b/colcon.meta @@ -4,9 +4,9 @@ "cmake-args": [ "-DRMW_UXRCE_MAX_NODES=1", "-DRMW_UXRCE_MAX_PUBLISHERS=11", - "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=4", - "-DRMW_UXRCE_MAX_SERVICES=5", - "-DRMW_UXRCE_MAX_CLIENTS=0", + "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=6", + "-DRMW_UXRCE_MAX_SERVICES=6", + "-DRMW_UXRCE_MAX_CLIENTS=1", "-DRMW_UXRCE_MAX_HISTORY=4", "-DRMW_UXRCE_TRANSPORT=custom", "-DRMW_UXRCE_ENTITY_CREATION_TIMEOUT=100", @@ -14,4 +14,4 @@ ] } } -} \ No newline at end of file +} diff --git a/compose.yaml b/compose.yaml new file mode 100644 index 0000000..a62c655 --- /dev/null +++ b/compose.yaml @@ -0,0 +1,6 @@ +services: + pio: + build: . + volumes: + - .:/app + # command: tail -f /dev/null \ No newline at end of file diff --git a/include/microros.hpp b/include/microros.hpp index 2877927..eb1051d 100644 --- a/include/microros.hpp +++ b/include/microros.hpp @@ -27,6 +27,8 @@ constexpr const char *WHEELS_COMMAND_TOPIC_NAME = "_motors_cmd"; constexpr const char *BATTERY_TOPIC_NAME = "battery"; constexpr const char *SERVOS_COMMAND_TOPIC_NAME = "cmd_ser"; +constexpr const char *GET_CPU_ID_SERVICE_NAME = "/get_cpu_id"; + enum AgentStates { WAITING_AGENT, AGENT_AVAILABLE, @@ -67,9 +69,12 @@ bool init_button_publishers(); bool init_led_subscribers(); bool init_param_server(); bool init_parameters(); +bool init_services(); bool publish_imu_msg(sensor_msgs__msg__Imu *imu_msg); bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg); bool publish_battery_msg(sensor_msgs__msg__BatteryState *msg); bool publish_range_msg(sensor_msgs__msg__Range *msg, uint8_t id); bool publish_button_msg(std_msgs__msg__Bool *msg, uint8_t id); + +void get_cpu_id_service_callback(const void *request, void *response); \ No newline at end of file diff --git a/platformio.ini b/platformio.ini index 8bc21fe..2a8fddb 100644 --- a/platformio.ini +++ b/platformio.ini @@ -15,13 +15,13 @@ extra_scripts = post:gen_hex.py pre:ignore_packages.py -board_microros_distro = humble -board_microros_transport = custom -board_microros_user_meta = colcon.meta - lib_deps = https://github.com/husarion/micro_ros_platformio lib_compat_mode = off +board_microros_distro = humble +board_microros_transport = custom +board_microros_user_meta = colcon.meta + upload_protocol = stlink debug_tool = stlink \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 5ce8d93..1e7b9db 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -197,6 +197,7 @@ int main() ThisThread::sleep_for(100); sens_power = 1; // sensors power on ThisThread::sleep_for(100); + init_battery(); init_wheels(); init_button_and_attach_to_callbacks(&button1, button1_rise_callback, button1_fall_callback); diff --git a/src/microros.cpp b/src/microros.cpp index 4af7a68..7722842 100644 --- a/src/microros.cpp +++ b/src/microros.cpp @@ -1,4 +1,5 @@ #include +#include rclc_executor_t executor; rclc_support_t support; @@ -18,10 +19,15 @@ rcl_subscription_t wheels_command_sub; rcl_subscription_t servos_command_sub; rcl_subscription_t led_subs[LED_COUNT]; +rcl_service_t get_cpu_id_service; + rclc_parameter_server_t param_server; std_msgs__msg__Bool led_msg; +std_srvs__srv__Trigger_Request get_cpu_id_service_request; +std_srvs__srv__Trigger_Response get_cpu_id_service_response; + const char *range_pub_names[] = {"range/fr", "range/fl", "range/rr", "range/rl"}; const char *buttons_pub_names[] = {"button/left", "button/right"}; const char *led_subs_names[] = {"led/left", "led/right"}; @@ -42,15 +48,16 @@ bool microros_init() { RCCHECK(rclc_node_init_default(&node, NODE_NAME, "", &support)); if (not init_wheels_command_subscriber() or - not init_servos_command_subscriber() or not init_wheels_state_publisher() or + not init_servos_command_subscriber() or not init_imu_publisher() or not init_battery_publisher() or not init_range_publishers() or not init_button_publishers() or not init_led_subscribers() or not init_param_server() or - not init_parameters()) { + not init_parameters() or + not init_services()) { return false; } @@ -59,7 +66,7 @@ bool microros_init() { RCCHECK(rclc_timer_init_default(&range_timer, &support, RCL_MS_TO_NS( 200 ), publish_range_sensors)); - RCCHECK(rclc_executor_init(&executor, &support.context, 11, &rcl_allocator)); + RCCHECK(rclc_executor_init(&executor, &support.context, 12, &rcl_allocator)); RCCHECK(rclc_executor_add_timer(&executor, &timer)); RCCHECK(rclc_executor_add_timer(&executor, &range_timer)); @@ -72,6 +79,7 @@ bool microros_init() { RCCHECK(rclc_executor_add_subscription(&executor, &led_subs[1], &led_msg, &led2_callback, ON_NEW_DATA)); RCCHECK(rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed)); + RCCHECK(rclc_executor_add_service(&executor, &get_cpu_id_service, &get_cpu_id_service_request, &get_cpu_id_service_response, get_cpu_id_service_callback)); RCCHECK(rclc_executor_prepare(&executor)); RCCHECK(rmw_uros_sync_session(1000)); @@ -97,6 +105,7 @@ bool microros_deinit() { RCCHECK(rcl_publisher_fini(&buttons_pubs[i], &node)); } RCCHECK(rclc_parameter_server_fini(¶m_server, &node)); + RCCHECK(rclc_executor_fini(&executor)); RCCHECK(rcl_node_fini(&node)); return true; @@ -208,6 +217,20 @@ bool init_parameters(){ return true; } +bool init_services() { + std_srvs__srv__Trigger_Request__init(&get_cpu_id_service_request); + std_srvs__srv__Trigger_Response__init(&get_cpu_id_service_response); + + RCCHECK(rclc_service_init_default( + &get_cpu_id_service, + &node, + ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger), + GET_CPU_ID_SERVICE_NAME + )); + + return true; +} + bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg) { RCCHECK(rcl_publish(&wheels_state_pub, msg, NULL)); return true; @@ -231,4 +254,31 @@ bool publish_range_msg(sensor_msgs__msg__Range *msg, uint8_t id) { bool publish_button_msg(std_msgs__msg__Bool *msg, uint8_t id) { RCCHECK(rcl_publish(&buttons_pubs[id], msg, NULL)); return true; -} \ No newline at end of file +} + +void get_cpu_id_service_callback(const void *req, void *res) { + (void)req; // Unused parameter + + const uint32_t ADDRESS = 0x1FFF7A10; + const uint8_t NUM_BYTES = 12; + uint8_t buffer[NUM_BYTES]; + memcpy(buffer, (void *)ADDRESS, NUM_BYTES); + + // Prepare the CPU ID in hexadecimal format + char cpu_id_buffer[NUM_BYTES * 2 + 1] = {0}; + char *hex_ptr = cpu_id_buffer; + for (uint8_t i = 0; i < NUM_BYTES; ++i) { + snprintf(hex_ptr, 3, "%02X", buffer[i]); + hex_ptr += 2; + } + + // Prepare the final output buffer with "CPU ID: " prefix + static char out_buffer[100]; // Ensure this is large enough + snprintf(out_buffer, sizeof(out_buffer), "{\"cpu_id\": \"%s\"}", cpu_id_buffer); + + // Set the response + std_srvs__srv__Trigger_Response *response = (std_srvs__srv__Trigger_Response *)res; + response->success = true; + response->message.data = out_buffer; + response->message.size = strlen(out_buffer); +}