74 lines
2.0 KiB
Arduino
74 lines
2.0 KiB
Arduino
#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);
|
|
}
|