[SummerSchool] MicroROS Topic Publisher With USound

小鱼

MicroROS超声波话题发布

#include <Arduino.h>
#include <micro_ros_platformio.h>

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

// 添加头文件
#include <std_msgs/msg/float32.h>
#define Trig 27 // 设定SR04连接的Arduino引脚
#define Echo 21
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

// 声明话题发布者
rcl_publisher_t publisher;
// 声明消息文件
std_msgs__msg__Float32 pub_msg;

// 定义定时器接收回调函数
void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL)
  {
    rcl_publish(&publisher, &pub_msg, NULL);
  }
}

void setup()
{
  pinMode(Trig, OUTPUT); // 初始化舵机和超声波
  pinMode(Echo, INPUT);  // 要检测引脚上输入的脉冲宽度，需要先设置为输入状态
  Serial.begin(115200);
  // 设置通过串口进行MicroROS通信
  set_microros_serial_transports(Serial);
  // 延时时一段时间，等待设置完成
  delay(2000);
  // 初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 创建初始化选项
  rclc_support_init(&support, 0, NULL, &allocator);
  // 创建节点 topic_sub_test
  rclc_node_init_default(&node, "topic_pub_test", "", &support);
  // 者初始化
  rclc_publisher_init_default(
      &publisher,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
      "distance");

  // 创建定时器，200ms发一次
  const unsigned int timer_timeout = 200;
  rclc_timer_init_default(
      &timer,
      &support,
      RCL_MS_TO_NS(timer_timeout),
      timer_callback);

  // 创建执行器
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  // 给执行器添加定时器
  rclc_executor_add_timer(&executor, &timer);
}



void loop()
{

  delay(100);
  // 循环处理数据
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  static double mtime;
  digitalWrite(Trig, LOW); // 测量距离
  delayMicroseconds(2);    // 延时2us
  digitalWrite(Trig, HIGH); 
  delayMicroseconds(10); // 产生一个10us的高脉冲去触发SR04
  digitalWrite(Trig, LOW);
  mtime = pulseIn(Echo, HIGH);                  // 检测脉冲宽度，注意返回值是微秒us
  float detect_distance = mtime / 58.0 / 100.0; // 计算出距离,输出的距离的单位是厘米cm
  pub_msg.data = detect_distance;
  // Serial.printf("distance=%f\n", detect_distance);

}

发布订阅同时进行

#include <Arduino.h>
#include <micro_ros_platformio.h>

#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <ESP32Servo.h>

Servo servo1; // 创建对象




// 添加头文件
#include <std_msgs/msg/float32.h>
#include <std_msgs/msg/int32.h>
#define Trig 27 // 设定SR04连接的Arduino引脚
#define Echo 21
rclc_executor_t executor;
rclc_support_t support;
rcl_allocator_t allocator;
rcl_node_t node;
rcl_timer_t timer;

// 声明话题订阅者
rcl_subscription_t subscriber;
// 声明消息文件
std_msgs__msg__Int32 sub_msg;
// 声明话题发布者
rcl_publisher_t publisher;
// 声明消息文件
std_msgs__msg__Float32 pub_msg;

// 定义定时器接收回调函数
void timer_callback(rcl_timer_t *timer, int64_t last_call_time)
{
  RCLC_UNUSED(last_call_time);
  if (timer != NULL)
  {
    rcl_publish(&publisher, &pub_msg, NULL);
  }
}

// 定义话题接收回调函数
void callback_subscription_(const void *msgin)
{
  const std_msgs__msg__Int32 *msg = (const std_msgs__msg__Int32 *)msgin;
  servo1.write(msg->data);          // 设置
}


void setup()
{
  pinMode(Trig, OUTPUT); // 初始化舵机和超声波
  pinMode(Echo, INPUT);  // 要检测引脚上输入的脉冲宽度，需要先设置为输入状态
  Serial.begin(115200);
  // 设置通过串口进行MicroROS通信
  set_microros_serial_transports(Serial);
  // 延时时一段时间，等待设置完成
  delay(2000);
  // 初始化内存分配器
  allocator = rcl_get_default_allocator();
  // 创建初始化选项
  rclc_support_init(&support, 0, NULL, &allocator);
  // 创建节点 topic_sub_test
  rclc_node_init_default(&node, "topic_pub_test", "", &support);
  // 者初始化
  rclc_publisher_init_default(
      &publisher,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32),
      "distance");
 // 订阅者初始化
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Int32),
      "servo_angle");
  // 创建定时器，200ms发一次
  const unsigned int timer_timeout = 200;
  rclc_timer_init_default(
      &timer,
      &support,
      RCL_MS_TO_NS(timer_timeout),
      timer_callback);

  // 创建执行器
  rclc_executor_init(&executor, &support.context, 2, &allocator);
  // 为执行器添加一个订阅者
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &callback_subscription_, ON_NEW_DATA);
  // 给执行器添加定时器
  rclc_executor_add_timer(&executor, &timer);
  servo1.setPeriodHertz(50);   // 舵机控制周期为50hz，即一个周期1000/50=20ms
  servo1.attach(4, 500, 2500); // 使用GPIO4作为舵机1信号引脚，占空比为500-2500us即 0.5-2.5ms
  

}



void loop()
{
  delay(100);
  // 循环处理数据
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  static double mtime;
  digitalWrite(Trig, LOW); // 测量距离
  delayMicroseconds(2);    // 延时2us
  digitalWrite(Trig, HIGH); 
  delayMicroseconds(10); // 产生一个10us的高脉冲去触发SR04
  digitalWrite(Trig, LOW);
  mtime = pulseIn(Echo, HIGH);                  // 检测脉冲宽度，注意返回值是微秒us
  float detect_distance = mtime / 58.0 / 100.0; // 计算出距离,输出的距离的单位是厘米cm
  pub_msg.data = detect_distance;
  // Serial.printf("distance=%f\n", detect_distance);

}

小鱼

控制测试

#!/usr/bin/env python3
from std_msgs.msg import Float32
from std_msgs.msg import Int32
import rclpy
from rclpy.node import Node

class NodeSubscribe02(Node):
    def __init__(self,name):
        super().__init__(name)
        self.get_logger().info("大家好，我是%s!" % name)
        # 创建订阅者
        self.command_subscribe_ = self.create_subscription(Float32,"distance",self.command_callback,10)
        self.command_publisher_ = self.create_publisher(Int32,"servo_angle", 10) 

    def command_callback(self,msg):
        angle = 0.0
        if msg.data>0.20:
            angle = 0
        else:
            angle = 90
        self.get_logger().info(f'收到距离：[{msg.data}]，发送角度：{angle}')
        
        pub_msg = Int32()
        pub_msg.data = angle
        self.command_publisher_.publish(pub_msg)



def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = NodeSubscribe02("topic_subscribe_02")  # 新建一个节点
    rclpy.spin(node) # 保持节点运行，检测是否收到退出指令（Ctrl+C）
    rclpy.shutdown() # 关闭rclpy


main()