I'm working on a robotics project where I have my ESP32 on a mobile robot and I want to create an application so I can send ROS2 vmd_vel data from my Ubuntu PC to the mobile robot via WiFi. So far I've created a WiFi server firmware for the ESP32 and the client application on my PC.
I managed to send strings from the client to the server using socket.h (later I have to change it so I can send cmd_vel data instead). I can receive the message on the server (ESP32) and create and send a response. For me the confusing part is that I can't output the received strings on the serial monitor on the server properly. It doesn't work reliably. Also when I receive the answer on the client and try to output it, it always shows way too many characters.
I used Arduino IDE to write and upload the code for the ESP32. I installed the ESP32 addon for Arduino IDE by Espressif Systems version 3.0.2.
Client code:
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");
}
}
You can see on the left side that the client prints the same response message multiple times and on the right side the server doesn't print his received messages at all. Why is this the case?
Thanks for any help. Best regards
Tobias