Skip to content
Snippets Groups Projects

Branch oscar

Closed Nicolas Federico Furquez Morena requested to merge branch_oscar into branch_nico
1 file
+ 76
47
Compare changes
  • Side-by-side
  • Inline
@@ -17,6 +17,7 @@
@@ -17,6 +17,7 @@
#include <ros.h>
#include <ros.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/UInt32.h>
#include <std_msgs/Bool.h>
#include <std_msgs/Bool.h>
 
#include <std_msgs/String.h>
#include <SPI.h>
#include <SPI.h>
#include "DW1000Ranging.h"
#include "DW1000Ranging.h"
@@ -26,6 +27,9 @@
@@ -26,6 +27,9 @@
#include <localization_localino/Distance.h>
#include <localization_localino/Distance.h>
#include "LocalinoHardware.h"
#include "LocalinoHardware.h"
 
#define turnOn(led) (digitalWrite(led, HIGH))
 
#define turnOff(led) (digitalWrite(led, LOW))
 
// ======================= ROS ==========================
// ======================= ROS ==========================
ros::NodeHandle_<LocalinoHardware> _nh;
ros::NodeHandle_<LocalinoHardware> _nh;
@@ -33,15 +37,16 @@ ros::NodeHandle_<LocalinoHardware> _nh;
@@ -33,15 +37,16 @@ ros::NodeHandle_<LocalinoHardware> _nh;
void ROS_newAntennaDelayValue(const std_msgs::UInt32& value);
void ROS_newAntennaDelayValue(const std_msgs::UInt32& value);
void ROS_doRange(const std_msgs::Bool& value);
void ROS_doRange(const std_msgs::Bool& value);
const unsigned int BUFFER_SIZE = 100;
std_msgs::String str_msg;
std_msgs::String str_msg;
localization_localino::Distance distancia;
localization_localino::Distance distancia;
ros::Publisher localization("localization", &distancia);
ros::Publisher localization("localization", &distancia);
 
ros::Publisher localization_status("localization_status", &str_msg);
 
ros::Subscriber<std_msgs::UInt32> subscriber("antenna_delay_param", &ROS_newAntennaDelayValue);
ros::Subscriber<std_msgs::UInt32> subscriber("antenna_delay_param", &ROS_newAntennaDelayValue);
ros::Subscriber<std_msgs::Bool> subscriberDoRange("do_range_param", &ROS_doRange);
ros::Subscriber<std_msgs::Bool> subscriberDoRange("do_range_param", &ROS_doRange);
char _bufferSerial[BUFFER_SIZE];
bool enviar = false;
 
bool doUpdateAD = false;
// ======================================================
// ======================================================
@@ -50,24 +55,28 @@ char anchorid[90] = "02:00:5B:D5:A9:9A:E2:9C"; // defines the ID of the An
@@ -50,24 +55,28 @@ char anchorid[90] = "02:00:5B:D5:A9:9A:E2:9C"; // defines the ID of the An
String hostip = "255.255.255.255";
String hostip = "255.255.255.255";
uint16_t _currentShortAddress;
uint16_t _currentShortAddress;
uint32_t _antennaDelay = 16469;
uint16_t _antennaDelay = 16469;
 
 
static const unsigned int LEN_MSG_BUFFER = 300;
 
char _msgBuffer[LEN_MSG_BUFFER];
bool enviar = false;
void setup() {
void setup() {
//Serial.begin(115200);
//Serial.begin(115200);
Serial1.begin(115200);
Serial1.begin(115200);
// indicate if we are an anchor
// indicate if we are an anchor
pinMode(LocalinoHardware::LED2, OUTPUT);
pinMode(LocalinoHardware::LED1, OUTPUT);
pinMode(LocalinoHardware::LED1, OUTPUT);
 
pinMode(LocalinoHardware::LED2, OUTPUT);
delay(1000);
//delay(1000);
DW1000Ranging_setup();
DW1000Ranging_setup();
 
ROS_setup();
 
enviarStatus("FIN SETUP");
 
 
//turnOn(LocalinoHardware::LED2);
// finished
// finished
digitalWrite(LocalinoHardware::LED2, HIGH);
digitalWrite(LocalinoHardware::LED1, HIGH);
}
}
void DW1000Ranging_setup() {
void DW1000Ranging_setup() {
@@ -90,8 +99,6 @@ void DW1000Ranging_setup() {
@@ -90,8 +99,6 @@ void DW1000Ranging_setup() {
byte* shortAddress = DW1000Ranging.getCurrentShortAddress();
byte* shortAddress = DW1000Ranging.getCurrentShortAddress();
_currentShortAddress = shortAddress[1] * 256 + shortAddress[0];
_currentShortAddress = shortAddress[1] * 256 + shortAddress[0];
ROS_setup();
}
}
void ROS_setup() {
void ROS_setup() {
@@ -99,64 +106,72 @@ void ROS_setup() {
@@ -99,64 +106,72 @@ void ROS_setup() {
_nh.subscribe(subscriber);
_nh.subscribe(subscriber);
_nh.subscribe(subscriberDoRange);
_nh.subscribe(subscriberDoRange);
_nh.advertise(localization);
_nh.advertise(localization);
 
_nh.advertise(localization_status);
 
}
}
void loop() {
void loop() {
if (enviar) {
DW1000Ranging.loop();
DW1000Ranging.loop();
}
_nh.spinOnce();
_nh.spinOnce();
delay(1);
delay(1);
}
}
void newRange() {
void newRange() {
// indicate TX
// indicate TX
digitalWrite(LocalinoHardware::LED2, HIGH);
turnOn(LocalinoHardware::LED2);
//Mando al serial como si fuera desde un anchor
if (enviar) {
unsigned long timestamp = millis();
//Mando al serial como si fuera desde un anchor
float tagRange = DW1000Ranging.getDistantDevice()->getRange();
unsigned long timestamp = millis();
float tagPower = DW1000Ranging.getDistantDevice()->getRXPower();
float tagRange = DW1000Ranging.getDistantDevice()->getRange();
String msg = "R," + String(DW1000Ranging.getDistantDevice()->getShortAddress(), HEX) + "," + String(2) + "," + tagRange + "," + String(_antennaDelay);
float tagPower = DW1000Ranging.getDistantDevice()->getRXPower();
//String msg = "R," + String(DW1000Ranging.getDistantDevice()->getShortAddress(), HEX) + "," + String(2) + "," + tagRange + "," + String(_antennaDelay);
Serial1.println("SendToUDP " + String(hostip) + ",10000," + msg);
sprintf(_msgBuffer, "R,%s,2,%f,%d", String(DW1000Ranging.getDistantDevice()->getShortAddress(), HEX).c_str(), tagRange, DW1000.getAntennaDelay());
enviarSerialRos(DW1000Ranging.getDistantDevice()->getShortAddress(), tagRange, tagPower, 0, msg);
// indicate TX off
//Serial1.println("SendToUDP " + String(hostip) + ",10000," + msg);
digitalWrite(LocalinoHardware::LED2, LOW);
enviarSerialRos(DW1000Ranging.getDistantDevice()->getShortAddress(), tagRange, tagPower, 0, _msgBuffer);
}
} else {
 
delay(1);
 
}
void newDevice(DW1000Device* device) {
if (doUpdateAD) {
 
DW1000.setAntennaDelay(_antennaDelay);
 
DW1000.commitConfiguration();
 
doUpdateAD = false;
 
}
String msg = "TAG [" + String(_currentShortAddress, HEX) + "] -> N,ranging init; 1 device added! -> short: " + String(device->getShortAddress(), HEX);
// indicate TX off
enviarSerialRos(1, 0.0 , 0.0, 1, msg);
turnOff(LocalinoHardware::LED2);
Serial1.println("SendToUDP " + String(hostip) + ",10000," + msg);
}
}
void newBlink(DW1000Device* device) {
void newBlink(DW1000Device* device) {
String msg = "ANCHOR [" + String(_currentShortAddress, HEX) + "] -> B,blink; 1 device added! -> short: " + String(device->getShortAddress(), HEX);
//String msg = "ANCHOR [" + String(_currentShortAddress, HEX) + "] -> B,blink; 1 device added! -> short: " + String(device->getShortAddress(), HEX);
sprintf(_msgBuffer, "ANCHOR [%s]-> B,blink; 1 device added! -> short: %s", String(_currentShortAddress, HEX).c_str(), String(device->getShortAddress(), HEX).c_str());
Serial1.println("SendToUDP " + String(hostip) + ",10000," + msg);
enviarStatus("newBlink");
 
//Serial1.println("SendToUDP " + String(hostip) + ",10000," + msg);
}
}
void inactiveDevice(DW1000Device* device) {
void inactiveDevice(DW1000Device* device) {
String msg = "ANCHOR [" + String(_currentShortAddress, HEX) + "] -> D,delete inactive device: " + String(device->getShortAddress(), HEX);
//String msg = "ANCHOR [" + String(_currentShortAddress, HEX) + "] -> D,delete inactive device: " + String(device->getShortAddress(), HEX);
sprintf(_msgBuffer, "ANCHOR [%s]-> D,delete inactive device! -> short: %s", String(_currentShortAddress, HEX).c_str(), String(device->getShortAddress(), HEX).c_str());
Serial1.println("SendToUDP " + String(hostip) + ",10000," + msg);
Serial1.println("SendToUDP " + String(hostip) + ",10000," + _msgBuffer);
 
enviarStatus("Se baja el tag");
}
}
void newLog(const char* log) {
void newLog(const char* log) {
String msg = "ANCHOR [" + String(_currentShortAddress, HEX) + "] -> " + log;
//String msg = "ANCHOR [" + String(_currentShortAddress, HEX) + "] -> " + log;
 
sprintf(_msgBuffer, "ANCHOR [%s] -> %s", String(_currentShortAddress, HEX).c_str(), log);
//Serial.println(msg);
//Serial.println(msg);
Serial1.println("SendToUDP " + String(hostip) + ",10000," + msg);
Serial1.println("SendToUDP " + String(hostip) + ",10000," + _msgBuffer);
}
}
void enviarSerialRos(int anchor, float distance, float power, int code, String mensaje) {
void enviarSerialRos(int anchor, float distance, float power, int code, const char* msg) {
mensaje.toCharArray(_bufferSerial, BUFFER_SIZE);
distancia.msg = msg;
distancia.msg = _bufferSerial;
distancia.anchor = anchor ;
distancia.anchor = anchor ;
distancia.code = code;
distancia.code = code;
distancia.distance = distance;
distancia.distance = distance;
@@ -165,16 +180,30 @@ void enviarSerialRos(int anchor, float distance, float power, int code, String m
@@ -165,16 +180,30 @@ void enviarSerialRos(int anchor, float distance, float power, int code, String m
localization.publish( &distancia );
localization.publish( &distancia );
}
}
void ROS_doRange(const std_msgs::Bool& value) {
void enviarStatus(const char* msg) {
enviar = value;
 
str_msg.data = msg ;
 
 
localization_status.publish( &str_msg );
}
}
 
void ROS_doRange(const std_msgs::Bool& value) {
 
 
enviar = value.data;
 
 
if (enviar) {
 
enviarStatus("Se empieza a enviar");
 
} else {
 
enviarStatus("Se para el envio.");
 
}
 
}
void ROS_newAntennaDelayValue(const std_msgs::UInt32& value) {
void ROS_newAntennaDelayValue(const std_msgs::UInt32& value) {
digitalWrite(LocalinoHardware::LED1, LOW);
turnOn(LocalinoHardware::LED1);
 
_antennaDelay = value.data;
_antennaDelay = value.data;
DW1000.setAntennaDelay(_antennaDelay);
doUpdateAD = true;
DW1000.commitConfiguration();
delay(500);
delay(500);
digitalWrite(LocalinoHardware::LED1, HIGH);
 
turnOff(LocalinoHardware::LED1);
}
}
Loading