Pico-ROS · g4sp3r · Aug 19, 2025 · Aug 17, 2025 · Aug 18, 2025 · Aug 18, 2025
diff --git a/.gitignore b/.gitignore
@@ -1,3 +1,5 @@
 build/
 .cache/
 docs/generated/
+**/venv/
+**/__pycache__/
diff --git a/.gitmodules b/.gitmodules
@@ -4,3 +4,6 @@
 [submodule "thirdparty/Micro-CDR"]
 	path = thirdparty/Micro-CDR
 	url = https://github.com/eProsima/Micro-CDR
+[submodule "tools/type-gen/rosidl"]
+	path = tools/type-gen/rosidl
+	url = https://github.com/ros2/rosidl.git
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -58,29 +58,31 @@ target_include_directories(picoparams PUBLIC
 if (TYPE_FILE)
   message("USER_TYPE_FILE: ${TYPE_FILE}")
   target_compile_definitions(picoserdes PUBLIC -DUSER_TYPE_FILE="${TYPE_FILE}")
-endif()
-target_link_libraries(picoros zenohpico::lib)
-target_link_libraries(picoserdes microcdr)
-target_link_libraries(picoparams zenohpico::lib microcdr picors picoserdes)
-
-# Add test executable
-if(PICOROS_BUILD_TESTS AND TYPE_FILE)
+  endif()
+  target_link_libraries(picoros zenohpico::lib)
+  target_link_libraries(picoserdes microcdr)
+  target_link_libraries(picoparams zenohpico::lib microcdr picors picoserdes)
+  
+  # Add test executable
+  if(PICOROS_BUILD_TESTS AND TYPE_FILE)
   message("User types serdes tests enabled")
   add_executable(test_user_types_serdes test/test_picoserdes.c)
   target_include_directories(test_user_types_serdes PRIVATE src)
   target_link_libraries(test_user_types_serdes PRIVATE picoserdes microcdr)
   add_test(NAME test_user_types_serdes COMMAND test_user_types)
-endif()
-
-# Build examples if enabled
-if(PICOROS_BUILD_EXAMPLES)
-  add_library(examples_serdes STATIC
-    src/picoserdes.c
-    src/picoserdes.h
-  )
-  target_include_directories(examples_serdes PUBLIC examples)
-  target_compile_definitions(examples_serdes PUBLIC -DUSER_TYPE_FILE="example_types.h")
-  target_link_libraries(examples_serdes microcdr)
+  endif()
+
+  # Build examples if enabled
+  if(PICOROS_BUILD_EXAMPLES)
+    add_library(examples_serdes STATIC
+      src/picoserdes.c
+      src/picoserdes.h
+    )
+    target_compile_definitions(picoros PUBLIC -DPICOROS_DEBUG)
+
+    target_include_directories(examples_serdes PUBLIC examples)
+    target_compile_definitions(examples_serdes PUBLIC -DUSER_TYPE_FILE="example_types.h")
+    target_link_libraries(examples_serdes microcdr)
 
   # Add test executable
   if(PICOROS_BUILD_TESTS)
@@ -116,6 +118,10 @@ if(PICOROS_BUILD_EXAMPLES)
   target_include_directories(talkerpp PUBLIC ${EXAMPLE_INCLUDE})
   target_link_libraries(talkerpp PRIVATE  ${EXAMPLE_LIBS})
 
+  add_executable(jointState_publisherpp examples/jointState_publisher.cpp  ${EXAMPLE_SRC})
+  target_include_directories(jointState_publisherpp PUBLIC ${EXAMPLE_INCLUDE})
+  target_link_libraries(jointState_publisherpp PRIVATE  ${EXAMPLE_LIBS})
+
   add_executable(listener examples/listener.c  ${EXAMPLE_SRC})
   target_include_directories(listener PUBLIC ${EXAMPLE_INCLUDE})
   target_link_libraries(listener PRIVATE  ${EXAMPLE_LIBS})
@@ -128,6 +134,10 @@ if(PICOROS_BUILD_EXAMPLES)
   target_include_directories(odometry_publisher PUBLIC ${EXAMPLE_INCLUDE})
   target_link_libraries(odometry_publisher PRIVATE  ${EXAMPLE_LIBS})
 
+  add_executable(batteryState_publisher examples/batteryState_publisher.c  ${EXAMPLE_SRC})
+  target_include_directories(batteryState_publisher PUBLIC ${EXAMPLE_INCLUDE})
+  target_link_libraries(batteryState_publisher PRIVATE  ${EXAMPLE_LIBS})
+
   add_executable(params_server examples/params_server.c  ${EXAMPLE_SRC})
   target_include_directories(params_server PUBLIC ${EXAMPLE_INCLUDE})
   target_link_libraries(params_server PRIVATE  ${EXAMPLE_LIBS})

diff --git a/examples/batteryState_publisher.c b/examples/batteryState_publisher.c
@@ -0,0 +1,94 @@
+/*******************************************************************************
+ * @file    odometry_publisher.c
+ * @brief   Example odometry publisher node for picoros
+ * @date    2025-May-27
+ * 
+ * @details This example demonstrates a ROS publisher node that publishes
+ *          simulated odometry messages on the "odom" topic.
+ * 
+ * @copyright Copyright (c) 2025 Ubiquity Robotics
+ *******************************************************************************/
+
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdint.h>
+#include "picoros.h"
+#include "picoserdes.h"
+
+// Use command line arguments to change default values
+#define MODE        "client"
+#define LOCATOR     "tcp/192.168.1.16:7447"
+
+// Common utils
+extern int picoros_parse_args(int argc, char **argv, picoros_interface_t* ifx);
+
+// Example Publisher
+picoros_publisher_t pub_bs = {
+    .topic = {
+        .name = "battery_state",
+        .type = ROSTYPE_NAME(ros_BatteryState),
+        .rihs_hash = ROSTYPE_HASH(ros_BatteryState),
+    },
+};
+
+// Example node
+picoros_node_t node = {
+    .name = "talker",
+};
+
+// Buffer for publication, used from this thread
+uint8_t pub_buf[1024];
+
+void publish_odometry(){
+    ros_BatteryState bat = {
+        .present = true,
+        .location = "main",
+        .serial_number = "ABCD1234",
+        .design_capacity = 20000,
+        .capacity = 18000,
+        .percentage = 50,
+        .charge = 9000,
+        .current = 0.5f,
+        .voltage = 25.1f,
+        .cell_voltage = {.data = (float[]){12.5f, 12.6f}, .n_elements = 2},
+        .cell_temperature = {.data = (float[]){31.2f, 32.5f}, .n_elements = 2},
+    };
+    printf("Publishing battery state...\n");
+    size_t len = ps_serialize(pub_buf, &bat, 1024);
+    if (len > 0){
+        picoros_publish(&pub_bs, pub_buf, len);
+    }
+    else{
+        printf("Message serialization error.");
+    }
+}
+
+int main(int argc, char **argv){
+    picoros_interface_t ifx = {
+        .mode = MODE,
+        .locator = LOCATOR,
+    };
+    int ret = picoros_parse_args(argc, argv , &ifx);
+
+    if(ret != 0){
+        return ret;
+    }
+
+    printf("Starting pico-ros interface %s %s\n", ifx.mode, ifx.locator );
+    while (picoros_interface_init(&ifx) == PICOROS_NOT_READY){
+        printf("Waiting RMW init...\n");
+        z_sleep_s(1);
+    }
+
+    printf("Starting Pico-ROS node %s domain:%d\n", node.name, node.domain_id);
+    picoros_node_init(&node);
+
+    printf("Declaring publisher on %s\n", pub_bs.topic.name);
+    picoros_publisher_declare(&node, &pub_bs);
+
+    while(true){
+        publish_odometry();
+        z_sleep_s(1);
+    }
+    return 0;
+}