ESP32 Forum

Code: Select all

// Copyright 2016 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

// for publisher
#include <chrono> 
#include <string>
// for subscriber
#include <functional>
#include <memory>

// for both
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

// for TCP client (https://www.geeksforgeeks.org/socket-programming-in-cpp/)
// additional sources: https://www.thegeekstuff.com/2011/12/c-socket-programming/
#include <stdio.h>
#include <iostream> 
#include <stdlib.h>
#include <unistd.h> 
#include <cstring> 
#include <sys/types.h>
#include <sys/socket.h> 
#include <netinet/in.h> 
#include <netdb.h>
#include <arpa/inet.h>



using std::placeholders::_1; // for subscriber
using namespace std::chrono_literals; // for publisher

class Mrobo_Client : public rclcpp::Node
{
public:
  int clientSocket;
  const char* server_ip = "192.168.178.49";
  const char* hostname = "mobile-robot";
  int port = 55348;
  struct sockaddr_in srvAddress;
  struct hostent *server;

  Mrobo_Client() // MinimalSubscriber
  : Node("mrobo_client"), count_(0) // minimal_publisher
  {
    int ret_code;
  
    // creating the client socket
    clientSocket = socket(AF_INET, SOCK_STREAM, 0);
    if(clientSocket < 0)
    {
        RCLCPP_INFO(this->get_logger(), "ERROR: Could not create socket. Code: %d", clientSocket);
        exit(1);
    } else {
        RCLCPP_INFO(this->get_logger(), "INFO: Socket created! Code: %d", clientSocket);
    }

/*
    server = gethostbyaddr(server_ip, 4, AF_INET);
    if (server == NULL) {
        RCLCPP_INFO(this->get_logger(), "ERROR: no such host\n");
        exit(0);
    }
*/

    server = gethostbyname(hostname);
    if (server == NULL) {
        RCLCPP_INFO(this->get_logger(), "ERROR: no such host\n");
        exit(0);
    }

    // defining the server address:
    bzero((char *) &srvAddress, sizeof(srvAddress));  // https://www.linuxhowtos.org/data/6/client.c
    srvAddress.sin_family = AF_INET;
    srvAddress.sin_port = htons(port);
    ret_code = inet_aton(server_ip, &srvAddress.sin_addr);

    // connecting to server
    RCLCPP_INFO(this->get_logger(), "Connecting...");    
    ret_code = connect(clientSocket, (struct sockaddr*)&srvAddress, sizeof(srvAddress));
    if( ret_code < 0)
    {
       RCLCPP_INFO(this->get_logger(), "Connect failed. Code: %d", ret_code);
    } else {
       RCLCPP_INFO(this->get_logger(), "Connected! Code: %d", ret_code);
    }
  
    // subscriber
    subscription_ = this->create_subscription<std_msgs::msg::String>(
      "topic", 10, std::bind(&Mrobo_Client::topic_callback, this, _1));
      
    // publisher
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    timer_ = this->create_wall_timer(
      500ms, std::bind(&Mrobo_Client::timer_callback, this));        
  }

private:
  std::string message_s;
  char serverResponse[256];
  
// for subscriber
  void topic_callback(const std_msgs::msg::String & msg) //const
  {
    int ret_code;  
  
    RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str());
    message_s = msg.data + "\0";

    // sending data to server
    ret_code = send(clientSocket, message_s.c_str(), strlen(message_s.c_str()), 0);
    if (ret_code == -1) {
       RCLCPP_INFO(this->get_logger(), "ERROR: Send failed. Code: %d", ret_code);
    } else {
       RCLCPP_INFO(this->get_logger(), "INFO: Send %d bytes successfully.", ret_code);
    }
    
    // receive messages from the server
    recv(clientSocket, &serverResponse, sizeof(serverResponse), 0); 
    
    RCLCPP_INFO(this->get_logger(), "Server response (String): %s", serverResponse);
    RCLCPP_INFO(this->get_logger(), "Server response (HEX): %02x", (unsigned)serverResponse[0]);
  }
  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;


// for publisher
  void timer_callback()
  { 
    // publish data on topic
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::to_string(count_++);
    publisher_->publish(message);
  }
  
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char * argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<Mrobo_Client>());
  rclcpp::shutdown();
  return 0;
}

Code: Select all

#include "Arduino.h"
#include <WiFi.h>

const char* ssid = "my_WiFi_network";
const char* password = "12345";

const char* hostname = "mobile-robot";
int status = WL_IDLE_STATUS;

WiFiServer server(55348); 
WiFiClient client;

const int STRING_BUFFER_SIZE = 50;
char stringBuffer[STRING_BUFFER_SIZE];

bool readLine(Stream& stream)
{
  static int index;

  while(stream.available())
  {   
    char c = stream.read();
    
    if(c >= 32 && index < STRING_BUFFER_SIZE - 1)
    {
      stringBuffer[index++] = c;
    }
    else if(c == '\n' && index > 0)
    {
      stringBuffer[index] = '\0';
      index = 0;
      return true;
    }
  }
  return false;
}

void setup() {

  Serial.begin(115200);

  delay(1000);

  WiFi.setHostname(hostname);
  WiFi.begin(ssid, password);

  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.println("Connecting to WiFi..");
  }

  Serial.println("Connected to the WiFi network");
  Serial.print("IP: ");
  Serial.print(WiFi.localIP());
  Serial.print("\n");
  Serial.print("Hostname: ");
  Serial.print(WiFi.getHostname());
  Serial.print("\n");

  server.begin();
}

void loop() {
  
  WiFiClient client = server.available();
  if (client) {
    Serial.println("New client connected");
    while (client.connected()) {
      if (readLine(client) == true)
      {
        Serial.print("Command: ");
        Serial.print(stringBuffer);
        Serial.print("\n");
        delay(50);
      }

      // send a response to the client
      client.println(stringBuffer);

      delay(100);
    }
    client.stop();
    Serial.println("Client disconnected");
  } 
}

Code: Select all

int len = recv(clientSocket, &serverResponse, sizeof(serverResponse)-1, 0);
if(len >= 0) {
  serverResponse[len] = '\0';
}