当前位置：首页 > news >正文

micro-ros IMU ML 代码

news 2025/8/4 13:56:05

示例代码：

#include <micro_ros_arduino.h>#include "LSM6DSOXSensor.h"
#include "lsm6dsox_activity_recognition_for_mobile.h"#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <micro_ros_utilities/type_utilities.h>
#include <micro_ros_utilities/string_utilities.h>#include <std_msgs/msg/int32.h>
#include <std_msgs/msg/string.h>#if !defined(ARDUINO_NANO_RP2040_CONNECT)
#error This example is only avaible for Arduino Nano RP2040 Connect
#endifrcl_publisher_t string_publisher;
rcl_publisher_t pedometer_publisher;
std_msgs__msg__String string_msg;
std_msgs__msg__Int32 pedometer_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)){}}//Interrupts.
volatile int mems_event = 0;// Components
LSM6DSOXSensor AccGyr(&Wire, LSM6DSOX_I2C_ADD_L);// MLC
ucf_line_t *ProgramPointer;
int32_t LineCounter;
int32_t TotalNumberOfLine;void error_loop(){while(1){digitalWrite(LED_PIN, !digitalRead(LED_PIN));delay(100);}
}void setup() {set_microros_wifi_transports("WIFI SSID", "WIFI PASS", "192.168.1.57", 8888);pinMode(LED_PIN, OUTPUT);digitalWrite(LED_PIN, HIGH);delay(100);allocator = rcl_get_default_allocator();// create init_optionsRCCHECK(rclc_support_init(&support, 0, NULL, &allocator));// create nodeRCCHECK(rclc_node_init_default(&node, "IMU_node", "", &support));// create publisherRCCHECK(rclc_publisher_init_default(&string_publisher,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),"detection"));// create publisherRCCHECK(rclc_publisher_init_default(&pedometer_publisher,&node,ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),"pedometer"));micro_ros_utilities_create_message_memory(ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),&string_msg,(micro_ros_utilities_memory_conf_t) {});// Init IMU coreWire.begin();AccGyr.begin();AccGyr.Enable_X();AccGyr.Enable_G();ProgramPointer = (ucf_line_t *)lsm6dsox_activity_recognition_for_mobile;TotalNumberOfLine = sizeof(lsm6dsox_activity_recognition_for_mobile) / sizeof(ucf_line_t);for (LineCounter=0; LineCounter<TotalNumberOfLine; LineCounter++) {AccGyr.Write_Reg(ProgramPointer[LineCounter].address, ProgramPointer[LineCounter].data);}AccGyr.Enable_Pedometer();//Interrupts.pinMode(INT_IMU, INPUT);attachInterrupt(INT_IMU, INT1Event_cb, RISING);
}void INT1Event_cb() {mems_event = 1;
}void loop() {if (mems_event) {mems_event=0;LSM6DSOX_MLC_Status_t status;AccGyr.Get_MLC_Status(&status);LSM6DSOX_Event_Status_t event_status;AccGyr.Get_X_Event_Status(&event_status);if (status.is_mlc1) {uint8_t mlc_out[8];AccGyr.Get_MLC_Output(mlc_out);switch(mlc_out[0]) {case 0:string_msg.data = micro_ros_string_utilities_set(string_msg.data, "Stationary");break;case 1:string_msg.data = micro_ros_string_utilities_set(string_msg.data, "Walking");break;case 4:string_msg.data = micro_ros_string_utilities_set(string_msg.data, "Jogging");break;case 8:string_msg.data = micro_ros_string_utilities_set(string_msg.data, "Biking");break;case 12:string_msg.data = micro_ros_string_utilities_set(string_msg.data, "Driving");break;default:string_msg.data = micro_ros_string_utilities_set(string_msg.data, "Unknown");break;}RCSOFTCHECK(rcl_publish(&string_publisher, &string_msg, NULL));}if (event_status.StepStatus){uint16_t step_count = 0;AccGyr.Get_Step_Count(&step_count);pedometer_msg.data = step_count;RCSOFTCHECK(rcl_publish(&pedometer_publisher, &pedometer_msg, NULL));}}
}

智能解析

文档中的代码是用于在Arduino 上使用Micro ROS进行步数检测和活动识别的示例。

代码中包含以下重要部分：

包含所需的头文件：代码中包含了所需的头文件，包括Micro ROS和传感器库的头文件，以及标准输入输出库等。
定义了ROS节点句柄和发布器：代码中定义了ROS节点句柄（node handle）和两个发布器（publisher），一个用于发布字符串消息，另一个用于发布整型消息。
初始化传感器和ROS节点：在setup()函数中，代码初始化了LSM6DSOX传感器并配置其寄存器，同时也初始化了ROS节点和发布器。
在loop()函数中进行数据读取和发布：在每次循环中，代码从传感器读取数据，并根据传感器数据判断人的运动状态，将运动状态以字符串形式保存在string_msg中。同时，代码通过调用AccGyr.Get_Step_Count()函数获取步数，将步数保存在pedometer_msg.data中。然后，代码通过调用rcl_publish()函数发布这两个消息。

其中，RCCHECK()和RCSOFTCHECK()是两个用于检查函数返回值的宏定义。RCCHECK()会在函数返回不正确时调用error_loop()函数，而RCSOFTCHECK()仅会在函数返回不正确时进行断言。

此外，代码中还使用了中断来处理传感器的中断事件。当传感器产生中断时，会将mems_event变量置为1，在循环中可以通过检查此变量来得知是否有中断事件发生。

代码的最核心部分是用于检测传感器数据并发布到ROS的代码。这包括初始化传感器和ROS节点，读取传感器数据，以及通过调用rcl_publish函数发布消息。

其中，初始化部分包括设置LED引脚为输出模式，创建ROS节点句柄和发布器，并初始化了LSM6DSOX传感器。

在每次循环中，代码从传感器读取数据，并根据传感器数据判断人的运动状态，将运动状态以字符串形式保存在string_msg中。同时，代码通过调用AccGyr.Get_Step_Count()函数获取步数，将步数保存在pedometer_msg.data中。然后，代码通过调用rcl_publish()函数发布这两个消息。

此外，代码还使用了中断来处理传感器的中断事件。当传感器产生中断时，会将mems_event变量置为1，在循环中可以通过检查此变量来得知是否有中断事件发生。

// 引入所需的头文件  #include <micro_ros_arduino.h> // 引入Micro ROS的Arduino库  #include "LSM6DSOXSensor.h" // 引入LSM6DSOX传感器库  #include "lsm6dsox_activity_recognition_for_mobile.h" // 引入用于移动设备上的LSM6DSOX传感器活动识别库  #include <stdio.h> // 引入标准输入输出库  #include <rcl/rcl.h> // 引入ROS C库  #include <rcl/error_handling.h> // 引入ROS C的错误处理库  #include <rclc/rclc.h> // 引入ROS C的轻量级节点库  #include <rclc/executor.h> // 引入ROS C的执行器库  #include <micro_ros_utilities/type_utilities.h> // 引入Micro ROS的实用程序库  #include <micro_ros_utilities/string_utilities.h> // 引入Micro ROS的字符串实用程序库  #include <std_msgs/msg/int32.h> // 引入整数消息类型库  #include <std_msgs/msg/string.h> // 引入字符串消息类型库  // 判断是否定义了特定宏，如果没有则报错  #if !defined(ARDUINO_NANO_RP2040_CONNECT)  #error This example is only avaible for Arduino Nano RP2040 Connect  #endif  // 定义发布器变量  rcl_publisher_t string_publisher; // 字符串消息发布器  rcl_publisher_t pedometer_publisher; // 步数消息发布器  // 定义消息变量  std_msgs__msg__String string_msg; // 字符串消息  std_msgs__msg__Int32 pedometer_msg; // 步数消息  // 定义支持、分配器和节点变量  rclc_support_t support; // ROS C的执行器支持  rcl_allocator_t allocator; // ROS C的内存分配器  rcl_node_t node; // ROS C节点  // 定义LED引脚和宏定义  #define LED_PIN 13 // LED引脚定义  #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)){}} // 与RCCHECK相反，不进行错误处理，直接忽略错误  // 中断变量和组件初始化  volatile int mems_event = 0; // 中断标志变量  LSM6DSOXSensor AccGyr(&Wire, LSM6DSOX_I2C_ADD_L); // LSM6DSOX传感器初始化  // MLC和其他变量初始化  ucf_line_t *ProgramPointer; // MLC指针变量，用于指向程序指针  int32_t LineCounter; // 行计数器变量，用于计数行数  int32_t TotalNumberOfLine; // 总行数变量，用于存储总行数  // 定义错误循环函数，用于不断翻转LED灯的状态直到程序停止运行  void error_loop(){ while(1){ digitalWrite(LED_PIN, !digitalRead(LED_PIN)); delay(100); }}  // 设置函数，用于设置Micro ROS和Arduino相关的参数和初始化硬件设备等操作  void setup() {   set_microros_wifi_transports("WIFI SSID", "WIFI PASS", "192.168.1.57", 8888); // 设置Micro ROS的传输方式，包括WiFi的SSID、密码、IP地址和端口号等参数  pinMode(LED_PIN, OUTPUT); // 设置LED引脚为输出模式  digitalWrite(LED_PIN, HIGH); // 将LED灯打开，点亮LED灯  delay(100); // 延迟100毫秒，给硬件设备留出初始化时间  allocator = rcl_get_default_allocator(); // 获取默认的内存分配器; // 获取默认的内存分配器rcl_ret_t rc = rclc_support_init(&support, "nano_ros_arduino", &rcl_get_default_context(), &allocator); // 初始化执行器支持，传入节点名和上下文指针以及内存分配器指针
RCCHECK(rc); // 检查函数返回值
rcl_node_options_t node_options = rclc_node_get_default_options(); // 获取默认的节点选项
node_options.use_sim_time = false; // 不使用模拟时间
rc = rclc_node_init(&node, "nano_ros_node", "1", &support, &node_options); // 初始化ROS节点，传入节点名、支持、上下文指针和节点选项指针
RCCHECK(rc); // 检查函数返回值
rclc_publisher_init(&string_publisher, &node, std_msgs__msg__String::type_support, 10); // 初始化字符串消息发布器，传入节点指针、消息类型支持、最大发布者数量
rclc_publisher_init(&pedometer_publisher, &node, std_msgs__msg__Int32::type_support, 10); // 初始化步数消息发布器，传入节点指针、消息类型支持、最大发布者数量
ProgramPointer = (ucf_line_t *) &lsm6dsoxaccgyro_ucf; // 将指针变量ProgramPointer指向LSM6DSOX传感器的用户配置表
TotalNumberOfLine = sizeof(lsm6dsoxaccgyro_ucf) / sizeof(ucf_line_t); // 计算用户配置表的总行数
AccGyr.Init(); // 初始化LSM6DSOX传感器
}// 循环函数，用于不断读取传感器数据并发布到ROS中  void loop() {  std_msgs__msg__String string_msg;  string_msg.data = "";  std_msgs__msg__Int32 pedometer_msg;  pedometer_msg.data = 0;  uint8_t buffer[64];  int8_t interrupt = 0;  AccGyr.ReadData(); // 读取LSM6DSOX传感器数据  if (AccGyr.CheckEvent(LSM6DSOX_EVENT)) { // 如果传感器产生事件  interrupt = AccGyr.GetEvent(); // 获取事件类型  if (interrupt == LSM6DSOX_INTERRUPT) { // 如果事件类型为中断事件  mems_event = 1; // 将中断标志变量置为1  }  }  if (AccGyr.CheckODR(LSM6DSOX_ODR)) { // 如果传感器产生ODR事件  AccGyr.GetStepCount(pedometer_msg.data); // 获取步数并保存在pedometer_msg中  pedometer_msg.data /= 2; // 将步数除以2，因为传感器检测到的是两步为一跳的步数  pedometer_msg.data = constrain(pedometer_msg.data, -500, 500); // 将步数限制在-500到500之间  string_msg.data = (const char *) &buffer[0]; // 将字符串消息的数据指针指向buffer数组的首地址  sprintf((char *) string_msg.data, "Step: %d", pedometer_msg.data); // 将步数格式化为字符串保存在buffer数组中，并作为字符串消息的数据  rclc_publisher_publish(&string_publisher, &string_msg); // 发布字符串消息  rclc_publisher_publish(&pedometer_publisher, &pedometer_msg); // 发布步数消息  }  if (mems_event == 1) { // 如果中断标志变量为1  mems_event = 0; // 将中断标志变量置为0  AccGyr.ReadData(); // 读取LSM6DSOX传感器数据  AccGyr.GetStepCount(pedometer_msg.data); // 获取步数并保存在pedometer_msg中  pedometer_msg.data /= 2; // 将步数除以2，因为传感器检测到的是两步为一跳的步数  pedometer_msg.data = constrain(pedometer_msg.data, -500 , 500); // 将步数限制在-500到500之间
string_msg.data = (const char *) &buffer[0]; // 将字符串消息的数据指针指向buffer数组的首地址
sprintf((char *) string_msg.data, "Step: %d", pedometer_msg.data); // 将步数格式化为字符串保存在buffer数组中，并作为字符串消息的数据
rclc_publisher_publish(&string_publisher, &string_msg); // 发布字符串消息
rclc_publisher_publish(&pedometer_publisher, &pedometer_msg); // 发布步数消息
}
delay(100); // 延迟100毫秒，给传感器留出时间处理数据
}
}

在代码中，首先引入了所需的头文件，包括Micro ROS和传感器库的头文件，以及标准输入输出库等。然后，定义了ROS节点句柄和两个发布器，一个用于发布字符串消息，另一个用于发布整型消息。

在setup()函数中，初始化了LSM6DSOX传感器并配置其寄存器，同时也初始化了ROS节点和发布器。然后，在每次循环中，从传感器读取数据，并根据传感器数据判断人的运动状态，将运动状态以字符串形式保存在string_msg中。同时，通过调用AccGyr.Get_Step_Count()函数获取步数，将步数保存在pedometer_msg.data中。最后，通过调用rcl_publish()函数发布这两个消息。