Arduino line follower driven by ROS1

1#!/usr/bin/env python
2
3import rospy
4from std_msgs.msg import String
5
6def ir_state_callback(msg):
7    if msg.data == "00100":
8        led_pub.publish("forward")
9    elif msg.data == "00010":
10        led_pub.publish("right")
11    elif msg.data == "01000":
12        led_pub.publish("left")
13    elif msg.data == "00001":
14        led_pub.publish("cright")
15    elif msg.data == "10000":
16        led_pub.publish("ceft")
17    elif msg.data == "00011":
18        led_pub.publish("sright")
19    elif msg.data == "11000":
20        led_pub.publish("sleft")
21    elif msg.data == "00110":
22        led_pub.publish("mright")
23    elif msg.data == "01100":
24        led_pub.publish("mleft")
25    elif msg.data == "00000":
26        led_pub.publish("stop")
27
28rospy.init_node('arduino_led_control')
29
30rospy.Subscriber('/ir_state', String, ir_state_callback)
31led_pub = rospy.Publisher('/mov_state', String, queue_size=10)
32
33rospy.spin()

1#include <ros.h>
2#include <std_msgs/String.h>
3
4ros::NodeHandle nh;
5
6const int irPin1 = 2;
7const int irPin2 = 3;
8const int irPin3 = 4;
9const int irPin4 = 5;
10const int irPin5 = 6;
11
12const int ledPin = 13;     // LED pin
13
14std_msgs::String str_msg;
15ros::Publisher ir_pub("/ir_state", &str_msg);
16
17void movCallback(const std_msgs::String& cmd_msg) {
18  String command = cmd_msg.data;
19
20  if (command == "forward") {
21    digitalWrite(7, 0);
22    digitalWrite(8, 1);
23    digitalWrite(9, 0);
24    digitalWrite(10, 1);
25  } else if (command == "right") {
26    digitalWrite(7, 0);
27    digitalWrite(8, 0);
28    digitalWrite(9, 0);
29    digitalWrite(10, 1);
30  } else if (command == "left") {
31    digitalWrite(7, 0);
32    digitalWrite(8, 1);
33    digitalWrite(9, 0);
34    digitalWrite(10, 0);
35  } else if (command == "cright") {
36    digitalWrite(7, 0);
37    digitalWrite(8, 0);
38    digitalWrite(9, 0);
39    digitalWrite(10, 1);
40  } else if (command == "cleft") {
41    digitalWrite(7, 0);
42    digitalWrite(8, 1);
43    digitalWrite(9, 0);
44    digitalWrite(10, 0);
45  } else if (command == "sright") {
46    digitalWrite(7, 0);
47    digitalWrite(8, 0);
48    digitalWrite(9, 0);
49    digitalWrite(10, 1);
50  } else if (command == "sleft") {
51    digitalWrite(7, 0);
52    digitalWrite(8, 1);
53    digitalWrite(9, 0);
54    digitalWrite(10, 0);
55  } else if (command == "mright") {
56    digitalWrite(7, 0);
57    digitalWrite(8, 0);
58    digitalWrite(9, 0);
59    digitalWrite(10, 1);
60  } else if (command == "mleft") {
61    digitalWrite(7, 0);
62    digitalWrite(8, 1);
63    digitalWrite(9, 0);
64    digitalWrite(10, 0);
65  } else if (command == "stop") {
66    digitalWrite(7, 0);
67    digitalWrite(8, 0);
68    digitalWrite(9, 0);
69    digitalWrite(10, 0);
70  } else {
71    digitalWrite(7, 0);
72    digitalWrite(8, 0);
73    digitalWrite(9, 0);
74    digitalWrite(10, 0);
75    digitalWrite(11, 0);
76
77  }
78}
79
80ros::Subscriber<std_msgs::String> sub("mov_state", &movCallback);
81
82void setup() {
83  pinMode(irPin1, INPUT);
84  pinMode(irPin2, INPUT);
85  pinMode(irPin3, INPUT);
86  pinMode(irPin4, INPUT);
87  pinMode(irPin5, INPUT);
88
89  pinMode(ledPin, OUTPUT);
90
91  pinMode(7, OUTPUT);
92  pinMode(8, OUTPUT);
93  pinMode(9, OUTPUT);
94  pinMode(10, OUTPUT);
95
96  nh.initNode();
97  nh.advertise(ir_pub);
98  nh.subscribe(sub);
99}
100
101void loop() {
102  int irState1 = digitalRead(irPin1);
103  int irState2 = digitalRead(irPin2);
104  int irState3 = digitalRead(irPin3);
105  int irState4 = digitalRead(irPin4);
106  int irState5 = digitalRead(irPin5);
107
108  if (irState1 && irState2 && !irState3 && irState4 && irState5 ) {
109    str_msg.data = "00100";
110  } else if (irState1 && irState2 && irState3 && !irState4 && irState5 ) {
111    str_msg.data = "00010";
112  } else if (irState1 && !irState2 && irState3 && irState4 && irState5 ) {
113    str_msg.data = "01000";
114  } else if (!irState1 && !irState2 && !irState3 && !irState4 && !irState5 ) {
115    str_msg.data = "11111";
116  } else if (!irState1 && irState2 && irState3 && irState4 && irState5 ) {
117    str_msg.data = "10000";
118  } else if (irState1 && irState2 && irState3 && irState4 && !irState5 ) {
119    str_msg.data = "00001";
120  } else if (!irState1 && !irState2 && irState3 && irState4 && irState5 ) {
121    str_msg.data = "11000";
122  } else if (irState1 && irState2 && irState3 && !irState4 && !irState5 ) {
123    str_msg.data = "00011";
124  } else if (irState1 && !irState2 && !irState3 && irState4 && irState5 ) {
125    str_msg.data = "01100";
126  } else if (irState1 && irState2 && !irState3 && !irState4 && irState5 ) {
127    str_msg.data = "00110";
128  } else {
129    str_msg.data = "00000";
130  }
131
132  ir_pub.publish(&str_msg);
133  nh.spinOnce();
134
135  // Wait for incoming messages from ROS
136  delay(100);
137}