ros arduino

最新推荐文章于 2023-06-10 14:47:34 发布

两个人，一座城

最新推荐文章于 2023-06-10 14:47:34 发布

阅读量122

点赞数

文章标签：单片机

本文链接：https://blog.csdn.net/qq_58237226/article/details/127153329

版权

//

///

#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif

#include <Servo.h>
#include <ros.h>
#include <std_msgs/UInt16.h>

ros::NodeHandle nh;

Servo servo;
// 0 40 65 100
void servo_cb( const std_msgs::UInt16& cmd_msg){
switch(cmd_msg.data){
case 1:
servo.write(0); //set servo angle, should be from 0-180
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
delay(1000);
break;
case 2:
servo.write(35); //set servo angle, should be from 0-180
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
delay(1000);
break;
case 3:
servo.write(65); //set servo angle, should be from 0-180
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
delay(1000);
break;
case 4:
servo.write(100); //set servo angle, should be from 0-180
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
delay(1000);
break;
default:
servo.write(0); //set servo angle, should be from 0-180
digitalWrite(13, HIGH-digitalRead(13)); //toggle led
delay(1000);
break;
}
}

ros::Subscriber<std_msgs::UInt16> sub("servo", servo_cb);

void setup(){
pinMode(13, OUTPUT);

nh.initNode();
nh.subscribe(sub);

servo.attach(9); //attach it to pin 9
}

void loop(){
nh.spinOnce();
delay(1);
}
/
/
/
#include "ros/ros.h"
#include "std_msgs/UInt16.h"
#include <sstream>

int main(int argc, char *argv[])
{
setlocale(LC_ALL,"");
ros::init(argc,argv,"ros_servo");
ros::NodeHandle nh;
ros::Publisher pub = nh.advertise<std_msgs::UInt16>("servo",10);
std_msgs::UInt16 msg;
ros::Rate r(1);
while (ros::ok())
{
msg.data = 2;
pub.publish(msg);
ROS_INFO("发送的消息:%d",msg.data);
r.sleep();
ros::spinOnce();
}
return 0;
}