arduino代码

laser_ctl/laser_ctl.ino

@@ -0,0 +1,36 @@
+#include <ros.h>
+#include <std_msgs/UInt8.h>
+
+ros::NodeHandle nh;
+
+#define LASER_PIN 3
+
+uint8_t currentBrightness = 0;
+
+void laserCallback(const std_msgs::UInt8& msg) {
+    currentBrightness = msg.data;
+    analogWrite(LASER_PIN, currentBrightness);
+}
+
+ros::Subscriber<std_msgs::UInt8> sub("laser_brightness", &laserCallback);
+
+std_msgs::UInt8 feedbackMsg;
+ros::Publisher pub("laser_feedback", &feedbackMsg);
+
+void setup() {
+    pinMode(LASER_PIN, OUTPUT);
+    analogWrite(LASER_PIN, 0);
+    
+    nh.initNode();
+    nh.subscribe(sub);
+    nh.advertise(pub);
+}
+
+void loop() {
+    nh.spinOnce();
+    
+    feedbackMsg.data = currentBrightness;
+    pub.publish(&feedbackMsg);
+    
+    delay(20);
+}

laser_ctl/laser_servo_ctl/laser_servo_ctl.ino

@@ -0,0 +1,73 @@
+#include <ros.h>
+#include <std_msgs/String.h>
+
+ros::NodeHandle nh;
+
+#define LASER_PIN 3
+#define SERVO_PIN 5
+
+// 使用char数组或String存储状态
+String currentstate_laser = "close";
+String currentstate_servo = "close";
+
+// 激光回调：接收 "yes" 打开，其他关闭
+void laserCallback(const std_msgs::String& msg) {
+    currentstate_laser = msg.data;
+    if (currentstate_laser == "yes") {
+        analogWrite(LASER_PIN, 255);  // 激光全开
+    } else {
+        analogWrite(LASER_PIN, 0);    // 激光关闭
+    }
+}
+
+// 舵机回调：接收 "servo_1" 转到90度
+void servoCallback(const std_msgs::String& msg) {
+    currentstate_servo = msg.data;
+    if (currentstate_servo == "servo_1") {
+        analogWrite(SERVO_PIN, 23);   // 约90度（舵机PWM: 0.5ms~2.5ms对应0~180度）
+    } else {
+        analogWrite(SERVO_PIN, 0);     // 0度位置
+    }
+}
+
+// 修正：订阅者变量名不能重复，且类型名大小写要正确
+ros::Subscriber<std_msgs::String> sub_laser("/caim/target_recognition_result", &laserCallback);
+ros::Subscriber<std_msgs::String> sub_servo("payload_drop_cmd", &servoCallback);
+
+// 修正：发布者变量名不能重复
+std_msgs::String feedbackMsg_laser;
+ros::Publisher pub_laser("laser_feedback", &feedbackMsg_laser);
+
+std_msgs::String feedbackMsg_servo;
+ros::Publisher pub_servo("servo_feedback", &feedbackMsg_servo);
+
+void setup() {
+    pinMode(LASER_PIN, OUTPUT);
+    analogWrite(LASER_PIN, 0);
+    
+    pinMode(SERVO_PIN, OUTPUT);
+    analogWrite(SERVO_PIN, 0);
+    
+    nh.initNode();
+    
+    // 注册两个订阅者
+    nh.subscribe(sub_laser);
+    nh.subscribe(sub_servo);
+    
+    // 注册两个发布者
+    nh.advertise(pub_laser);
+    nh.advertise(pub_servo);
+}
+
+void loop() {
+    nh.spinOnce();
+    
+    // 发布反馈状态
+    feedbackMsg_laser.data = currentstate_laser.c_str();
+    pub_laser.publish(&feedbackMsg_laser);
+    
+    feedbackMsg_servo.data = currentstate_servo.c_str();
+    pub_servo.publish(&feedbackMsg_servo);
+    
+    delay(20);
+}