esp32发布机器人电池电压到ros2（micro-ros+CoCube）

Posted 2022-08-09 zhangrelay

示例程序测试端口ad数值：

#include "Arduino.h"
#include <ESP32AnalogRead.h>
ESP32AnalogRead adc;
void setup()

	adc.attach(34);
	Serial.begin(115200);


void loop()

	delay(50);
	Serial.println("Voltage = "+String(adc.readVoltage()));

这只是一个简单测试ad口电压的程序，没有滤波，没有依据实际进行数值变换。

34是测量电压端口。

初始化，ad和串口波特率：

void setup()

	adc.attach(34);
	Serial.begin(115200);

循环测电压并输出串口，时延50ms：

void loop()

	delay(50);
	Serial.println("Voltage = "+String(adc.readVoltage()));

这是单片机简单测电压的程序。

需要补充滤波和标度变换。留作思考题。

效果如下图： 

---

micro-ros发布一个消息案例如下：

#include <micro_ros_arduino.h>

#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>

#if !defined(ESP32) && !defined(TARGET_PORTENTA_H7_M7) && !defined(ARDUINO_NANO_RP2040_CONNECT)
#error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect and ESP32 Dev module
#endif

rcl_publisher_t publisher;
std_msgs__msg__Int32 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))


void error_loop()
  while(1)
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  


void timer_callback(rcl_timer_t * timer, int64_t last_call_time)

  RCLC_UNUSED(last_call_time);
  if (timer != NULL) 
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  


void setup() 
  set_microros_wifi_transports("***", "***", "***", 8888);

  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

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

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
    "topic_name"));

  msg.data = 0;


void loop() 
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;

---

需要修改哪些代码？

ROS1/2中的数据类型如下：

ROS Message Types
Bool
Byte
ByteMultiArray
Char
ColorRGBA
Duration
Empty
Float32
Float32MultiArray
Float64
Float64MultiArray
Header
Int16
Int16MultiArray
Int32
Int32MultiArray
Int64
Int64MultiArray
Int8
Int8MultiArray
MultiArrayDimension
MultiArrayLayout
String
Time
UInt16
UInt16MultiArray
UInt32
UInt32MultiArray
UInt64
UInt64MultiArray
UInt8
UInt8MultiArray

battery电池，电压情况：

高>4.1v为插入usb口，低为电池供电。

---

需要启动agent：

参考程序如下： 

#include <micro_ros_arduino.h>

#include <stdio.h>
#include <rcl/rcl.h>
#include <rcl/error_handling.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>

#include <std_msgs/msg/float32.h>

#if !defined(ESP32) && !defined(TARGET_PORTENTA_H7_M7) && !defined(ARDUINO_NANO_RP2040_CONNECT)
#error This example is only avaible for Arduino Portenta, Arduino Nano RP2040 Connect and ESP32 Dev module
#endif

#define BAT_DET 34

rcl_publisher_t publisher;
std_msgs__msg__Float32 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))


void error_loop()
  while(1)
    digitalWrite(LED_PIN, !digitalRead(LED_PIN));
    delay(100);
  


void timer_callback(rcl_timer_t * timer, int64_t last_call_time)

  RCLC_UNUSED(last_call_time);
  if (timer != NULL) 
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data++;
  


void setup() 
  set_microros_wifi_transports("***", "***", "***", 8888);

  pinMode(BAT_DET, INPUT);
  
  pinMode(LED_PIN, OUTPUT);
  digitalWrite(LED_PIN, HIGH);
  

  delay(2000);

  allocator = rcl_get_default_allocator();

  //create init_options
  RCCHECK(rclc_support_init(&support, 0, NULL, &allocator));

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

  // create publisher
  RCCHECK(rclc_publisher_init_best_effort(
    &publisher,
    &node,
    ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
    "robot_battery"));

  msg.data = 0.66;


void loop() 
    float battery = 4.21 * analogRead(BAT_DET) / 2435;
    RCSOFTCHECK(rcl_publish(&publisher, &msg, NULL));
    msg.data=battery;
    delay(1000);

---