Sending rostopic form esp32 through husarnet
I know that it's not so popular but maybe someone is using it. I'm trying to publish topic through husarnet to my robot, I've successfully made that in LAN but my application needs to run in different wifi networks.
Husarnet between devices with ros installed works perfectly and it don't needs any rosserial_node to be working so my question is how to subscribe topics from esp to device with ros.
Here is code of great working code in the same network:
#include <ros.h>
#include <std_msgs/String.h>
#include <WiFi.h>
#include <my_pkg/EspTrigger.h>
#define PUB_FREQ 5
#define ID 1
const char* ssid = "**********";
const char* password = "*********";
IPAddress server(***,***,***,***); // ip of your ROS server
IPAddress ip_address;
int status = WL_IDLE_STATUS;
const int motionSensor = 27;
// Timer: Auxiliary variables
unsigned long now = millis();
unsigned long lastTrigger = 0;
boolean startTimer = false;
WiFiClient client;
class WiFiHardware {
public:
WiFiHardware() {};
void init() {
// do your initialization here. this probably includes TCP server/client setup
client.connect(server, 11411);
}
// read a byte from the serial port. -1 = failure
int read() {
// implement this method so that it reads a byte from the TCP connection and returns it
// you may return -1 is there is an error; for example if the TCP connection is not open
return client.read(); //will return -1 when it will works
}
// write data to the connection to ROS
void write(uint8_t* data, int length) {
// implement this so that it takes the arguments and writes or prints them to the TCP connection
for(int i=0; i<length; i++)
client.write(data[i]);
}
// returns milliseconds since start of program
unsigned long time() {
return millis(); // easy; did this one for you
}
};
void setupWiFi()
{
WiFi.begin(ssid, password);
Serial.print("\nConnecting to "); Serial.println(ssid);
uint8_t i = 0;
while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
if(i == 21){
Serial.print("Could not connect to"); Serial.println(ssid);
while(1) delay(500);
}
Serial.print("Ready! Use ");
Serial.print(WiFi.localIP());
Serial.println(" to access client");
}
ros::NodeHandle_ <WiFiHardware> nh;
simulation::EspTrigger esp_msg;
//std_msgs::Bool bool_msg;
ros::Publisher motion_trigger("motion_trigger", &esp_msg);
volatile bool move = 0;
void IRAM_ATTR detectsMovement() {
Serial.println("MOTION DETECTED!!!");
::move = 1;
}
void setup() {
Serial.begin(115200);
setupWiFi();
delay(2000);
pinMode(motionSensor, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(motionSensor), detectsMovement, RISING);
nh.initNode();
nh.advertise(motion_trigger);
}
void loop() {
now = millis();
if((now - lastTrigger) > (1000/PUB_FREQ)) {
Serial.println("Motion stopped...");
startTimer = false;
esp_msg.move = ::move;
esp_msg.id = ID;
motion_trigger.publish( &esp_msg );
::move = 0;
lastTrigger = millis();
nh.spinOnce();
}void loop() {
now = millis();
if((now - lastTrigger) > (1000/PUB_FREQ)) {
Serial.println("Motion stopped...");
startTimer = false;
esp_msg.move = ::move;
esp_msg.id = ID;
motion_trigger.publish( &esp_msg );
::move = 0;
lastTrigger = millis();
nh.spinOnce();
}
}
}
To subscribe this topic i need to launch rosserial node:
rosrun rosserial_python serial_node.py tcp
My attempts to run this by husarnet are:
#include <ros.h>
#include <std_msgs/String.h>
#include <WiFi.h>
#include <Husarnet.h>
#include <simulation/EspTrigger.h>
#define PUB_FREQ 1
#define ID 1
const char* ssid = "******";
const char* password = "******";
int status = WL_IDLE_STATUS;
const int motionSensor = 27;
// Timer: Auxiliary variables
unsigned long now = millis();
unsigned long lastTrigger = 0;
boolean startTimer = false;
HusarnetClient client;
// Husarnet credentials
const char* hostName0 = "myesp32"; //this will be the name of the 1st ESP32 device at https://app.husarnet.com
const char* hostName1 = "rosmaster"; //this will be the name of the 2nd device at https://app.husarnet.com
const char* husarnetJoinCode = "fc94:b01d:1803:8dd8:b293:5c7d:7639:932a/dmXM7AwkroSwc8WRrSXkzF";
uint16_t port = 11411;
class WiFiHardware {
public:
WiFiHardware() {};
void init() {
// do your initialization here. this probably includes TCP server/client setup
client.connect(hostName1, port);
}
// read a byte from the serial port. -1 = failure
int read() {
// implement this method so that it reads a byte from the TCP connection and returns it
// you may return -1 is there is an error; for example if the TCP connection is not open
return client.read(); //will return -1 when it will works
}
// write data to the connection to ROS
void write(uint8_t* data, int length) {
// implement this so that it takes the arguments and writes or prints them to the TCP connection
for(int i=0; i<length; i++)
client.write(data[i]);
}
// returns milliseconds since start of program
unsigned long time() {
return millis(); // easy; did this one for you
}
};
void setupWiFi()
{
WiFi.mode(WIFI_MODE_APSTA);
WiFi.enableIpV6();
WiFi.begin(ssid, password);
Serial.print("\nConnecting to "); Serial.println(ssid);
uint8_t i = 0;
while (WiFi.status() != WL_CONNECTED && i++ < 20) delay(500);
if(i == 21){
Serial.print("Could not connect to"); Serial.println(ssid);
while(1) delay(500);
}
Serial.println("done");
Serial.print("IP address: ");
Serial.println(WiFi.localIPv6());
Husarnet.join(husarnetJoinCode, hostName0);
Husarnet.start();
}
ros::NodeHandle_ <WiFiHardware> nh;
simulation::EspTrigger esp_msg;
//std_msgs::Bool bool_msg;
ros::Publisher motion_trigger("motion_trigger", &esp_msg);
volatile bool move = 0;
void IRAM_ATTR detectsMovement() {
Serial.println("MOTION DETECTED!!!");
::move = 1;
}
void setup() {
Serial.begin(115200);
setupWiFi();
delay(2000);
pinMode(motionSensor, INPUT_PULLUP);
attachInterrupt(digitalPinToInterrupt(motionSensor), detectsMovement, RISING);
nh.initNode();
nh.advertise(motion_trigger);
xTaskCreate(
taskConnection, /* Task function. */
"taskConnection", /* String with name of task. */
10000, /* Stack size in bytes. */
NULL, /* Parameter passed as input of the task */
1, /* Priority of the task. */
NULL);
}
void loop() {
now = millis();
if((now - lastTrigger) > (1000/PUB_FREQ)) {
startTimer = false;
esp_msg.move = ::move;
esp_msg.id = ID;
motion_trigger.publish( &esp_msg );
::move = 0;
lastTrigger = millis();
nh.spinOnce();
}
}
void taskConnection( void * parameter ) {
while (1) {
Serial.printf("Client connected: %d\r\n", (int)client.connected());
}
client.stop();
Serial.println("Client disconnected.");
Serial.println("");
}
As far I know this should be automatically passed to my rosMaster but even if I can ping this device when i run rosserial node nothings happen. also return of "Client connected" is 0 .
Thanks in advice :)
Asked by khasreto on 2019-07-13 11:50:34 UTC
Comments
Ok I've found solution it was problem with rosserial, so made update for husarenet support. I've made also pull request but it is still waiting for acceptance if you want to try it here is link to my git repo:
https://github.com/adamkrawczyk/rosserial.git - it's under ipv6-husarnet branch.
Here is rest of source you may need to use ros+husarnet.
husarnet
https://www.hackster.io/donowak
husarion tutorials
Example usage of this rosserial pkg:
rosrun rosserial_python serial_node tcp 11412
rosrun rosserial_python serial_node.py _port:=tcp _tcp_port:=11413
Asked by khasreto on 2019-07-21 04:48:29 UTC