esp32与ros2的开关灯

Posted 2021-10-01 zhangrelay

单片机开关灯是必须的，如何告知ros2，这里用主题方式实现。需要先阅读：

• esp32与ros2的欢乐启程https://blog.csdn.net/ZhangRelay/article/details/120229431?spm=1001.2014.3001.5501

---

开关灯的示例如下：

#include <WiFi.h>

const char* ssid     = "yourssid";
const char* password = "yourpasswd";

WiFiServer server(80);

void setup()
{
    Serial.begin(115200);
    pinMode(5, OUTPUT);      // set the LED pin mode

    delay(10);

    // We start by connecting to a WiFi network

    Serial.println();
    Serial.println();
    Serial.print("Connecting to ");
    Serial.println(ssid);

    WiFi.begin(ssid, password);

    while (WiFi.status() != WL_CONNECTED) {
        delay(500);
        Serial.print(".");
    }

    Serial.println("");
    Serial.println("WiFi connected.");
    Serial.println("IP address: ");
    Serial.println(WiFi.localIP());
    
    server.begin();

}

int value = 0;

void loop(){
 WiFiClient client = server.available();   // listen for incoming clients

  if (client) {                             // if you get a client,
    Serial.println("New Client.");           // print a message out the serial port
    String currentLine = "";                // make a String to hold incoming data from the client
    while (client.connected()) {            // loop while the client's connected
      if (client.available()) {             // if there's bytes to read from the client,
        char c = client.read();             // read a byte, then
        Serial.write(c);                    // print it out the serial monitor
        if (c == '\\n') {                    // if the byte is a newline character

          // if the current line is blank, you got two newline characters in a row.
          // that's the end of the client HTTP request, so send a response:
          if (currentLine.length() == 0) {
            // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK)
            // and a content-type so the client knows what's coming, then a blank line:
            client.println("HTTP/1.1 200 OK");
            client.println("Content-type:text/html");
            client.println();

            // the content of the HTTP response follows the header:
            client.print("Click <a href=\\"/H\\">here</a> to turn the LED on pin 5 on.<br>");
            client.print("Click <a href=\\"/L\\">here</a> to turn the LED on pin 5 off.<br>");

            // The HTTP response ends with another blank line:
            client.println();
            // break out of the while loop:
            break;
          } else {    // if you got a newline, then clear currentLine:
            currentLine = "";
          }
        } else if (c != '\\r') {  // if you got anything else but a carriage return character,
          currentLine += c;      // add it to the end of the currentLine
        }

        // Check to see if the client request was "GET /H" or "GET /L":
        if (currentLine.endsWith("GET /H")) {
          digitalWrite(5, HIGH);               // GET /H turns the LED on
        }
        if (currentLine.endsWith("GET /L")) {
          digitalWrite(5, LOW);                // GET /L turns the LED off
        }
      }
    }
    // close the connection:
    client.stop();
    Serial.println("Client Disconnected.");
  }
}

这是示例代码，一定要注意！LED端口要与电路板一致，本开发板要修改为2。

pinMode(2, OUTPUT);      // set the LED pin mode

还有：

        if (currentLine.endsWith("GET /H")) {
          digitalWrite(2, HIGH);               // GET /H turns the LED on
        }
        if (currentLine.endsWith("GET /L")) {
          digitalWrite(2, LOW);                // GET /L turns the LED off
        }

这和ROS2有啥关系？完全没有啊，下面直接插入ROS2相关代码，源码如下：

#include <ros2arduino.h>

#include <WiFi.h>
#include <WiFiUdp.h>
#include <WiFiClient.h>
//#include <WebServer.h>
//#include <ESPmDNS.h>

#define SSID       "ESP32LoveROS2"
#define SSID_PW    "66666666"
#define AGENT_IP   "172.20.10.2"
#define AGENT_PORT 2021 //AGENT port number

#define LED 2
#define PUBLISH_FREQUENCY 1 //hz

char ledflag=2;

void publishString(std_msgs::String* msg, void* arg)
{
  (void)(arg);

  static int cnt = 0;
  if(ledflag==2){
    sprintf(msg->data, "欢乐的esp32和ros2 %d", cnt++);
  }
  else if(ledflag==0)
  {
    sprintf(msg->data, "欢乐的esp32灯灭了 %d", cnt++);
  }
  else if(ledflag==1)
  {
    sprintf(msg->data, "欢乐的esp32灯亮了 %d", cnt++);
  }
}

class StringPub : public ros2::Node
{
public:
  StringPub()
  : Node("esp32_pub_node")
  {
    ros2::Publisher<std_msgs::String>* publisher_ = this->createPublisher<std_msgs::String>("esp32_chatter");
    this->createWallFreq(PUBLISH_FREQUENCY, (ros2::CallbackFunc)publishString, nullptr, publisher_);
  }
};

WiFiUDP udp;

WiFiServer server(80);

void setup() 
{
  pinMode(LED, OUTPUT);
  WiFi.begin(SSID, SSID_PW);
  while(WiFi.status() != WL_CONNECTED);
  
  server.begin();

  ros2::init(&udp, AGENT_IP, AGENT_PORT);
  
}

void loop() 
{
  static StringPub StringNode;
  ros2::spin(&StringNode);
  
  WiFiClient client = server.available();   // listen for incoming clients
  if (client) {                             // if you get a client,
    String currentLine = "";                // make a String to hold incoming data from the client
    while (client.connected()) {            // loop while the client's connected
      if (client.available()) {             // if there's bytes to read from the client,
        char c = client.read();             // read a byte, then
        if (c == '\\n') {                    // if the byte is a newline character

          // if the current line is blank, you got two newline characters in a row.
          // that's the end of the client HTTP request, so send a response:
          if (currentLine.length() == 0) {
            // HTTP headers always start with a response code (e.g. HTTP/1.1 200 OK)
            // and a content-type so the client knows what's coming, then a blank line:
            client.println("HTTP/1.1 200 OK");
            client.println("Content-type:text/html");
            client.println();

            // the content of the HTTP response follows the header:
            client.print("Click <a href=\\"/H\\">here</a> to turn the LED on pin 2 on. cslg<br>");
            client.print("Click <a href=\\"/L\\">here</a> to turn the LED on pin 2 off. cslg<br>");
            client.print("Click <a href=\\"/Blink\\">here</a> to turn the LED on pin 2 off. cslg<br>");

            // The HTTP response ends with another blank line:
            client.println();
            // break out of the while loop:
            break;
          } else {    // if you got a newline, then clear currentLine:
            currentLine = "";
          }
        } else if (c != '\\r') {  // if you got anything else but a carriage return character,
          currentLine += c;      // add it to the end of the currentLine
        }

        // Check to see if the client request was "GET /H" or "GET /L":
        if (currentLine.endsWith("GET /H")) {
          digitalWrite(LED, HIGH);               // GET /H turns the LED on
        //  sprintf(msg->data, "灯亮了");
          ledflag=1;
        }
        if (currentLine.endsWith("GET /L")) {
          digitalWrite(LED, LOW);                // GET /L turns the LED off
        //  sprintf(msg->data, "灯灭了");
          ledflag=0;
        }
      }
    }
    // close the connection:
    client.stop();
  }
}

如果不能理解这段代码，先阅读本文最初的那个链接。

---

效果是如下的：

---

后续，小乌龟和机器人都会加入进来^_^ 

---