micro-ros IMU ML 代码

2023-08-09 15:44:27 浏览数 (2)

示例代码： 

代码语言：javascript复制
#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
#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;
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_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

  // create node
  RCCHECK(rclc_node_init_default(&node, "IMU_node", "", &support));

  // create publisher
  RCCHECK(rclc_publisher_init_default(
    &string_publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, String),
    "detection"));

  // create publisher
  RCCHECK(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 core
  Wire.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进行步数检测和活动识别的示例。

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

  1. 包含所需的头文件：代码中包含了所需的头文件，包括Micro ROS和传感器库的头文件，以及标准输入输出库等。
  2. 定义了ROS节点句柄和发布器：代码中定义了ROS节点句柄（node handle）和两个发布器（publisher），一个用于发布字符串消息，另一个用于发布整型消息。
  3. 初始化传感器和ROS节点：在setup()函数中，代码初始化了LSM6DSOX传感器并配置其寄存器，同时也初始化了ROS节点和发布器。
  4. 在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，在循环中可以通过检查此变量来得知是否有中断事件发生。

代码语言：javascript复制
// 引入所需的头文件  
            #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()函数发布这两个消息。

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

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

整个程序的流程是：在setup()函数中进行初始化，然后在每次循环中读取传感器数据、判断运动状态、获取步数、发布消息，并使用中断处理传感器中断事件。最后，通过delay()函数给传感器留出时间处理数据。

然后让ai写代码，写出来的也不怎么样呢。

看来程序还是太新了。

ros 变量 函数 数据 字符串

0 人点赞