重要提示
鱼香小铺正式开业,最低499可入手一台能建图会导航的移动机器人,淘宝搜店:鱼香ROS 或点击链接查看。
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
提问前必看的发帖注意事项—— 提问前必看!不符合要求的问题拒绝回答!!
社区使用指南—如何添加标签修改密码
[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()