From 86085411a857b2fc19eb370064d0ffdd16437ceb Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Tue, 14 Nov 2023 20:08:45 -0600 Subject: [PATCH 001/113] Initial code --- firmware/motor_control/motor_control.ino | 133 ++++++++++------------- 1 file changed, 55 insertions(+), 78 deletions(-) diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index 0851951d..99e5866d 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -4,52 +4,50 @@ #include "differential_drive.h" #include "motor_with_encoder.h" #include "common.h" -#include -#include - -//LEDs -const int ledPin = LED_BUILTIN; -const int CANLED = 0; -const int LED0 = 1; - -//Encoder Pins -const int encoderLeftA = 3; -const int encoderLeftB = 2; -const int encoderRightA = 5; -const int encoderRightB = 4; -//Motor PWM pins -const int leftMotorPwmPin = 16; -const int rightMotorPwmPin = 14; - -const int estopPin = 27; - -//SPI pins -static const byte MCP2515_SCK = 10; // SCK input of MCP2515 -static const byte MCP2515_MOSI = 11; // SDI input of MCP2515 -static const byte MCP2515_MISO = 8; // SDO output of MCP2515 -static const byte MCP2515_CS = 9; // CS input of MCP2515 -static const byte MCP2515_INT = 7; // INT output of MCP2515 - -ACAN2515 can(MCP2515_CS, SPI1, MCP2515_INT); -TickTwo motor_update_timer(setMotorUpdateFlag, 25); +//LED +static int LED1 = 22; +static int LED2 = 21; + +//ESTOP +static int ESTOP = 27; + +//PWM +static int PWM0 = 6; +static int PWM1 = 7; + +//ENCODER +static int ENC0A = 8; +static int ENC0B = 9; +static int ENC1A = 10; +static int ENC1B = 11; + +//EEPROM +static int EEPROMSDA = 0; +static int EEPROMSCL = 1; +static int EEPROMWP = 2; -static const uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL; // 8 MHz +//SPI +static int MCP2515_MISO = 16; +static int MCP2515_CS = 17; +static int MCP2515_SCK = 18; +static int MCP2515_MOSI = 19; +static int MCP215_INT = 20; + +//Quartz oscillator - 8MHz +static uint23_t QUARTZFREQUENCY = 8UL * 1000UL * 1000UL + +ACAN2515 can(MCP2515_CS, SPI0, MCP2515_INT); + +TickTwo motor_update_timer(setMotorUpdateFlag, 25); CANMessage frame; CANMessage outFrame; -// Setup CONBus variables -CONBus::CONBus conbus; -CONBus::CANBusDriver conbus_can(conbus, 0x10); // device id 0x10 (16) - robotStatus_t roboStatus; distance motorDistances; MotorCommand motorCommand; -// motor leftMotor(leftMotorPwmPin, true); -// motor rightMotor(rightMotorPwmPin, false); - MotorWithEncoder leftMotor(leftMotorPwmPin, encoderLeftA, encoderLeftB, true); MotorWithEncoder rightMotor(rightMotorPwmPin, encoderRightA, encoderRightB, false); DifferentialDrive drivetrain(leftMotor, rightMotor, 0.025); @@ -80,54 +78,26 @@ float desired_forward_velocity; float desired_angular_velocity; void setup() { - delay(50); + pinMode(ENC0A, INPUT); + pinMode(ENC0B, INPUT); + pinMode(ENC1A, INPUT); + pinMode(ENC1B, INPUT); - drivetrain.setup(); + pinMode(LED1, OUTPUT); + pinMode(LED2, OUTPUT); + pinMode(ESTOP, INPUT); - pinMode(encoderRightA, INPUT); - pinMode(encoderRightB, INPUT); - pinMode(encoderLeftA, INPUT); - pinMode(encoderLeftB, INPUT); + drivetrain.setup(); attachInterrupt(encoderLeftA, updateLeft, CHANGE); attachInterrupt(encoderRightA, updateRight, CHANGE); motor_update_timer.start(); -} -void setup1(){ - Serial.begin(9600); - delay(50); + configureCan(); - pinMode(LED0, OUTPUT); - pinMode(CANLED, OUTPUT); - pinMode(estopPin, INPUT); - - conbus.addReadOnlyRegister(0x00, drivetrain.getUpdatePeriod()); - conbus.addRegister(0x01, drivetrain.getPulsesPerRadian()); - conbus.addRegister(0x02, drivetrain.getWheelRadius()); - conbus.addRegister(0x03, drivetrain.getWheelbaseLength()); - conbus.addRegister(0x04, drivetrain.getSlewRateLimit()); - conbus.addRegister(0x05, drivetrain.getLeftEncoderFactor()); - conbus.addRegister(0x06, drivetrain.getRightEncoderFactor()); - - conbus.addRegister(0x10, drivetrain.getVelocitykP()); - conbus.addRegister(0x11, drivetrain.getVelocitykI()); - conbus.addRegister(0x12, drivetrain.getVelocitykD()); - conbus.addRegister(0x13, drivetrain.getVelocitykF()); - - conbus.addRegister(0x20, drivetrain.getAngularkP()); - conbus.addRegister(0x21, drivetrain.getAngularkI()); - conbus.addRegister(0x22, drivetrain.getAngularkD()); - conbus.addRegister(0x23, drivetrain.getAngularkF()); - - conbus.addRegister(0x30, &useObstacleAvoidance); - conbus.addRegister(0x31, &collisonBoxDist); - - conbus.addRegister(0x40, &sendStatistics); - - conbus.addRegister(0x50, &motor_updates_between_deltaodom); } + void loop() { updateTimers(); if (MOTOR_UPDATE_FLAG) { @@ -137,8 +107,6 @@ void loop() { MOTOR_UPDATE_FLAG = false; } -} -void loop1(){ if (motor_updates_in_deltaodom >= motor_updates_between_deltaodom) { motor_updates_in_deltaodom = 0; @@ -155,7 +123,12 @@ void loop1(){ conbus_can.popReply(); } } + + + + } + void configureCan() { SPI1.setSCK(MCP2515_SCK); SPI1.setTX(MCP2515_MOSI); @@ -237,9 +210,9 @@ void sendCanOdomMsgOut(){ outFrame.len = 4; memcpy(outFrame.data, &pid_controls, 4); can.tryToSend(outFrame); - } - + } } + void printCanMsg(CANMessage frame) { Serial.print(" id: "); Serial.println(frame.id, HEX); @@ -251,12 +224,15 @@ void printCanMsg(CANMessage frame) { } Serial.println(""); } + void updateLeft(){ drivetrain.pulseLeftEncoder(); } + void updateRight(){ drivetrain.pulseRightEncoder(); } + void resetDelta(){ motorDistances.xn = 0; motorDistances.yn = 0; @@ -265,6 +241,7 @@ void resetDelta(){ delta_y = 0; delta_theta = 0; } + void updateTimers() { motor_update_timer.update(); } From 1af31cfebad8b08f9ecfdbe4f49edb3e89b35b82 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Tue, 28 Nov 2023 19:59:48 -0600 Subject: [PATCH 002/113] Added ConbusParams --- firmware/motor_control/conbus_params.h | 111 +++++++++++++++++++++++ firmware/motor_control/motor_control.ino | 27 +++--- 2 files changed, 122 insertions(+), 16 deletions(-) create mode 100644 firmware/motor_control/conbus_params.h diff --git a/firmware/motor_control/conbus_params.h b/firmware/motor_control/conbus_params.h new file mode 100644 index 00000000..fb16c83c --- /dev/null +++ b/firmware/motor_control/conbus_params.h @@ -0,0 +1,111 @@ +#ifndef CONBUS_PARAMS +#define CONBUS_PARAMS + +#include +#include + +class ConbusParams { + +public: + ConbusParams(int SDA, int SCL, int WP); + + void update_variable(float id, float value); + void refresh(); + + float getUpdatePeriod(); + float getPulsesPerRadian(); + float getWheelRadius(); + float getWheelBaseLength(); + float getSlewRateLimit(); + float getLeftEncoderFactor(); + float getRightEncoderFactor(); + float getVelocitykP(); + float getVelocitykI(); + float getVelocitykD(); + float getVelocitykF(); + float getAngularkP(); + float getAngularkI(); + float getAngularkD(); + float getAngularkF(); + float getUseObstacleAvoidence(); + float getCollisionDist(); + float getSendStatistics(); + float getMotorUpdateBetweenDelatOdom(); + +private: + void refreshVariables(); + void setUpdatePeriod(float period); + void setPulsesPerRadian(float ppr); + void setWheelRadius(float radius); + void setWheelBaseLength(float length); + void setSlewRateLimit(float slewRate); + void setLeftEncoderFactor(float factor); + void setRightEncoderFactor(float factor); + void setVelocitykP(float vkp); + void setVelocitykI(float vki); + void setVelocitykD(float vkd); + void setVelocitykF(float vkf); + void setAngularkP(float akp); + void setAngularkI(float aki); + void setAngularkD(float akd); + void setAngularkF(float akf); + void setUseObstacleAvoidence(bool use); + void setCollisionDist(int distance); + void setSendStatistics(bool send); + void setMotorUpdateBetweenDeltaOdom(bool update); + + float updatePeriod; + float pulsesPerRadian; + float wheelRadius; + float wheelBaseLength; + float slewRateLimit; + float leftEncoderFactor; + float rightEncoderFactor; + float velocitykP; + float velocitykI; + float velocitykD; + float velocitykF; + float angularkP; + float angularkI; + float angularkD; + float angularkF; + bool useObstacleAvoidance; + bool sendStatisics; + bool MotorUpdaatesBetweenDeltaOdom; + int collisionDist; + + int addr_updatePeriod; + int addr_pulsesPerRadian; + int addr_wheelRadius; + int addr_wheelBaseLength; + int addr_slewRateLimit; + int addr_leftEncoderFactor; + int addr_rightEncoderFactor; + int addr_velocitykP; + int addr_velocitykI; + int addr_velocitykD; + int addr_velocitykF; + int addr_angularkP; + int addr_angularkI; + int addr_angularkD; + int addr_angularkF; + int addr_useObstacleAvoidance; + int addr_sendStatisics; + int addr_MotorUpdaatesBetweenDeltaOdom; + int addr_updatePeriod; + int addr_collisionDist; + + + int EEPROM_SDA; + int EEPROM_SCL; + int EEPROM_WP; + + +}; + +inline ConbusParams::ConbusParams(int SDA, int SCL, int WP) { + Wire.setSDA(SDA); + Wire.setSCL(SCL); + pinMode(WP, OUTPUT); + +} \ No newline at end of file diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index 99e5866d..299a7c11 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -13,14 +13,14 @@ static int LED2 = 21; static int ESTOP = 27; //PWM -static int PWM0 = 6; -static int PWM1 = 7; +static int PWM0 = 6; //right motor controller +static int PWM1 = 7; //left motor controller //ENCODER -static int ENC0A = 8; -static int ENC0B = 9; -static int ENC1A = 10; -static int ENC1B = 11; +static int ENC0A = 8; //right encoder +static int ENC0B = 9; //right encoder +static int ENC1A = 10; //left encoder +static int ENC1B = 11; //left encoder //EEPROM static int EEPROMSDA = 0; @@ -48,8 +48,8 @@ robotStatus_t roboStatus; distance motorDistances; MotorCommand motorCommand; -MotorWithEncoder leftMotor(leftMotorPwmPin, encoderLeftA, encoderLeftB, true); -MotorWithEncoder rightMotor(rightMotorPwmPin, encoderRightA, encoderRightB, false); +MotorWithEncoder leftMotor(PWM1, ENC1A, ENC1B, true); +MotorWithEncoder rightMotor(PWM0, ENC0A, ENC1B, false); DifferentialDrive drivetrain(leftMotor, rightMotor, 0.025); void configureCan(); @@ -89,13 +89,12 @@ void setup() { drivetrain.setup(); - attachInterrupt(encoderLeftA, updateLeft, CHANGE); - attachInterrupt(encoderRightA, updateRight, CHANGE); + attachInterrupt(ENC0A, updateLeft, CHANGE); + attachInterrupt(ENC1A, updateRight, CHANGE); motor_update_timer.start(); configureCan(); - } void loop() { @@ -123,10 +122,6 @@ void loop() { conbus_can.popReply(); } } - - - - } void configureCan() { @@ -161,7 +156,7 @@ void onCanRecieve() { switch (frame.id) { case 10: - motorCommand = *(MotorCommand*)(frame.data); //Noah made me cry. I dont know what they did but I dont like it one bit - Jorge + motorCommand = *(MotorCommand*)(frame.data); desired_forward_velocity = (float)motorCommand.setpoint_forward_velocity / SPEED_SCALE_FACTOR; desired_angular_velocity = (float)motorCommand.setpoint_angular_velocity / SPEED_SCALE_FACTOR; From da2b2e4942c530a0e5c47bc74fd28d5b7b03c3cb Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Tue, 28 Nov 2023 20:18:27 -0600 Subject: [PATCH 003/113] Added Addresses for EEPROM --- firmware/motor_control/conbus_params.h | 42 +++++++++++++------------- 1 file changed, 21 insertions(+), 21 deletions(-) diff --git a/firmware/motor_control/conbus_params.h b/firmware/motor_control/conbus_params.h index fb16c83c..0b6e53dc 100644 --- a/firmware/motor_control/conbus_params.h +++ b/firmware/motor_control/conbus_params.h @@ -74,26 +74,26 @@ class ConbusParams { bool MotorUpdaatesBetweenDeltaOdom; int collisionDist; - int addr_updatePeriod; - int addr_pulsesPerRadian; - int addr_wheelRadius; - int addr_wheelBaseLength; - int addr_slewRateLimit; - int addr_leftEncoderFactor; - int addr_rightEncoderFactor; - int addr_velocitykP; - int addr_velocitykI; - int addr_velocitykD; - int addr_velocitykF; - int addr_angularkP; - int addr_angularkI; - int addr_angularkD; - int addr_angularkF; - int addr_useObstacleAvoidance; - int addr_sendStatisics; - int addr_MotorUpdaatesBetweenDeltaOdom; - int addr_updatePeriod; - int addr_collisionDist; + int addr_updatePeriod = 0x00; + int addr_pulsesPerRadian = 0x04; + int addr_wheelRadius = 0x08; + int addr_wheelBaseLength = 0xC; + int addr_slewRateLimit = 0x10; + int addr_leftEncoderFactor = 0x14; + int addr_rightEncoderFactor = 0x18; + int addr_velocitykP = 0x1C; + int addr_velocitykI = 0x20; + int addr_velocitykD = 0x24; + int addr_velocitykF = 0x28; + int addr_angularkP = 0x2C; + int addr_angularkI = 0x30; + int addr_angularkD = 0x34; + int addr_angularkF = 0x38; + int addr_useObstacleAvoidance = 0x3C; + int addr_sendStatisics = 0x40; + int addr_MotorUpdaatesBetweenDeltaOdom = 0x44; + int addr_updatePeriod = 0x48; + int addr_collisionDist = 0x4C; int EEPROM_SDA; @@ -107,5 +107,5 @@ inline ConbusParams::ConbusParams(int SDA, int SCL, int WP) { Wire.setSDA(SDA); Wire.setSCL(SCL); pinMode(WP, OUTPUT); - + } \ No newline at end of file From 7cf23dcd8e3136893732a9ab8fcaf5013574e25b Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Tue, 23 Jan 2024 18:40:29 -0600 Subject: [PATCH 004/113] Updated pins on last year code --- firmware/motor_control/motor_control.ino | 76 ++++++++++++++++++------ 1 file changed, 57 insertions(+), 19 deletions(-) diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index 299a7c11..f5a4b353 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -4,6 +4,8 @@ #include "differential_drive.h" #include "motor_with_encoder.h" #include "common.h" +#include +#include //LED static int LED1 = 22; @@ -44,12 +46,19 @@ TickTwo motor_update_timer(setMotorUpdateFlag, 25); CANMessage frame; CANMessage outFrame; +// Setup CONBus variables +CONBus::CONBus conbus; +CONBus::CANBusDriver conbus_can(conbus, 0x10); // device id 0x10 (16) + robotStatus_t roboStatus; distance motorDistances; MotorCommand motorCommand; -MotorWithEncoder leftMotor(PWM1, ENC1A, ENC1B, true); -MotorWithEncoder rightMotor(PWM0, ENC0A, ENC1B, false); +// motor leftMotor(leftMotorPwmPin, true); +// motor rightMotor(rightMotorPwmPin, false); + +MotorWithEncoder leftMotor(leftMotorPwmPin, encoderLeftA, encoderLeftB, true); +MotorWithEncoder rightMotor(rightMotorPwmPin, encoderRightA, encoderRightB, false); DifferentialDrive drivetrain(leftMotor, rightMotor, 0.025); void configureCan(); @@ -78,25 +87,54 @@ float desired_forward_velocity; float desired_angular_velocity; void setup() { - pinMode(ENC0A, INPUT); - pinMode(ENC0B, INPUT); - pinMode(ENC1A, INPUT); - pinMode(ENC1B, INPUT); - - pinMode(LED1, OUTPUT); - pinMode(LED2, OUTPUT); - pinMode(ESTOP, INPUT); + delay(50); drivetrain.setup(); - attachInterrupt(ENC0A, updateLeft, CHANGE); - attachInterrupt(ENC1A, updateRight, CHANGE); + pinMode(encoderRightA, INPUT); + pinMode(encoderRightB, INPUT); + pinMode(encoderLeftA, INPUT); + pinMode(encoderLeftB, INPUT); + + attachInterrupt(encoderLeftA, updateLeft, CHANGE); + attachInterrupt(encoderRightA, updateRight, CHANGE); motor_update_timer.start(); - - configureCan(); } +void setup1(){ + Serial.begin(9600); + delay(50); + configureCan(); + + pinMode(LED0, OUTPUT); + pinMode(CANLED, OUTPUT); + pinMode(estopPin, INPUT); + + conbus.addReadOnlyRegister(0x00, drivetrain.getUpdatePeriod()); + conbus.addRegister(0x01, drivetrain.getPulsesPerRadian()); + conbus.addRegister(0x02, drivetrain.getWheelRadius()); + conbus.addRegister(0x03, drivetrain.getWheelbaseLength()); + conbus.addRegister(0x04, drivetrain.getSlewRateLimit()); + conbus.addRegister(0x05, drivetrain.getLeftEncoderFactor()); + conbus.addRegister(0x06, drivetrain.getRightEncoderFactor()); + + conbus.addRegister(0x10, drivetrain.getVelocitykP()); + conbus.addRegister(0x11, drivetrain.getVelocitykI()); + conbus.addRegister(0x12, drivetrain.getVelocitykD()); + conbus.addRegister(0x13, drivetrain.getVelocitykF()); + + conbus.addRegister(0x20, drivetrain.getAngularkP()); + conbus.addRegister(0x21, drivetrain.getAngularkI()); + conbus.addRegister(0x22, drivetrain.getAngularkD()); + conbus.addRegister(0x23, drivetrain.getAngularkF()); + conbus.addRegister(0x30, &useObstacleAvoidance); + conbus.addRegister(0x31, &collisonBoxDist); + + conbus.addRegister(0x40, &sendStatistics); + + conbus.addRegister(0x50, &motor_updates_between_deltaodom); +} void loop() { updateTimers(); if (MOTOR_UPDATE_FLAG) { @@ -106,6 +144,8 @@ void loop() { MOTOR_UPDATE_FLAG = false; } +} +void loop1(){ if (motor_updates_in_deltaodom >= motor_updates_between_deltaodom) { motor_updates_in_deltaodom = 0; @@ -123,7 +163,6 @@ void loop() { } } } - void configureCan() { SPI1.setSCK(MCP2515_SCK); SPI1.setTX(MCP2515_MOSI); @@ -156,7 +195,7 @@ void onCanRecieve() { switch (frame.id) { case 10: - motorCommand = *(MotorCommand*)(frame.data); + motorCommand = *(MotorCommand*)(frame.data); //Noah made me cry. I dont know what they did but I dont like it one bit - Jorge desired_forward_velocity = (float)motorCommand.setpoint_forward_velocity / SPEED_SCALE_FACTOR; desired_angular_velocity = (float)motorCommand.setpoint_angular_velocity / SPEED_SCALE_FACTOR; @@ -205,9 +244,9 @@ void sendCanOdomMsgOut(){ outFrame.len = 4; memcpy(outFrame.data, &pid_controls, 4); can.tryToSend(outFrame); - } -} + } +} void printCanMsg(CANMessage frame) { Serial.print(" id: "); Serial.println(frame.id, HEX); @@ -240,4 +279,3 @@ void resetDelta(){ void updateTimers() { motor_update_timer.update(); } - From e40b798716250ad0751f039afca63e17879f6300 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Tue, 23 Jan 2024 19:09:11 -0600 Subject: [PATCH 005/113] Fixed pins --- firmware/motor_control/motor_control.ino | 26 ++++++++++++------------ 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index f5a4b353..09cbd317 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -54,11 +54,11 @@ robotStatus_t roboStatus; distance motorDistances; MotorCommand motorCommand; -// motor leftMotor(leftMotorPwmPin, true); -// motor rightMotor(rightMotorPwmPin, false); +// motor leftMotor(PWM1, true); +// motor rightMotor(PWM0, false); -MotorWithEncoder leftMotor(leftMotorPwmPin, encoderLeftA, encoderLeftB, true); -MotorWithEncoder rightMotor(rightMotorPwmPin, encoderRightA, encoderRightB, false); +MotorWithEncoder leftMotor(PWM1, ENC1A, ENC1B, true); +MotorWithEncoder rightMotor(PWM0, ENC0A, ENC0B, false); DifferentialDrive drivetrain(leftMotor, rightMotor, 0.025); void configureCan(); @@ -91,13 +91,13 @@ void setup() { drivetrain.setup(); - pinMode(encoderRightA, INPUT); - pinMode(encoderRightB, INPUT); - pinMode(encoderLeftA, INPUT); - pinMode(encoderLeftB, INPUT); + pinMode(ENC0A, INPUT); + pinMode(ENC0B, INPUT); + pinMode(ENC1A, INPUT); + pinMode(ENC1B, INPUT); - attachInterrupt(encoderLeftA, updateLeft, CHANGE); - attachInterrupt(encoderRightA, updateRight, CHANGE); + attachInterrupt(ENC1A, updateLeft, CHANGE); + attachInterrupt(ENC0A, updateRight, CHANGE); motor_update_timer.start(); } @@ -106,9 +106,9 @@ void setup1(){ delay(50); configureCan(); - pinMode(LED0, OUTPUT); - pinMode(CANLED, OUTPUT); - pinMode(estopPin, INPUT); + pinMode(LED1, OUTPUT); + pinMode(LED2, OUTPUT); + pinMode(ESTOP, INPUT); conbus.addReadOnlyRegister(0x00, drivetrain.getUpdatePeriod()); conbus.addRegister(0x01, drivetrain.getPulsesPerRadian()); From 30203e4b3341be08279664d7305c98c40ab5d040 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Tue, 23 Jan 2024 19:22:03 -0600 Subject: [PATCH 006/113] Fixed CAN variable --- firmware/motor_control/motor_control.ino | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index 09cbd317..7976e8d1 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -164,11 +164,11 @@ void loop1(){ } } void configureCan() { - SPI1.setSCK(MCP2515_SCK); - SPI1.setTX(MCP2515_MOSI); - SPI1.setRX(MCP2515_MISO); - SPI1.setCS(MCP2515_CS); - SPI1.begin(); + SPI0.setSCK(MCP2515_SCK); + SPI0.setTX(MCP2515_MOSI); + SPI0.setRX(MCP2515_MISO); + SPI0.setCS(MCP2515_CS); + SPI0.begin(); Serial.println("Configure ACAN2515"); @@ -195,7 +195,7 @@ void onCanRecieve() { switch (frame.id) { case 10: - motorCommand = *(MotorCommand*)(frame.data); //Noah made me cry. I dont know what they did but I dont like it one bit - Jorge + motorCommand = *(MotorCommand*)(frame.data); desired_forward_velocity = (float)motorCommand.setpoint_forward_velocity / SPEED_SCALE_FACTOR; desired_angular_velocity = (float)motorCommand.setpoint_angular_velocity / SPEED_SCALE_FACTOR; From 56ab5d3e0e17492fc3ceafa68edee0df53ed8b23 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Wed, 24 Jan 2024 19:49:54 -0600 Subject: [PATCH 007/113] Got serial to Arduino IDE to work --- firmware/motor_control/CANBusDriver.h | 146 ++++++++++++++++++++ firmware/motor_control/CONBus.h | 165 +++++++++++++++++++++++ firmware/motor_control/motor_control.ino | 46 ++++--- 3 files changed, 340 insertions(+), 17 deletions(-) create mode 100644 firmware/motor_control/CANBusDriver.h create mode 100644 firmware/motor_control/CONBus.h diff --git a/firmware/motor_control/CANBusDriver.h b/firmware/motor_control/CANBusDriver.h new file mode 100644 index 00000000..35789d87 --- /dev/null +++ b/firmware/motor_control/CANBusDriver.h @@ -0,0 +1,146 @@ +#ifndef CANBUS_DRIVER_H +#define CANBUS_DRIVER_H + +#include +#include +#include + +#include "CONBus.h" + +namespace CONBus{ + +typedef struct{ + uint8_t registerAddress; +} CAN_readRegisterMessage; + +typedef struct{ + uint8_t registerAddress; + uint8_t length; + uint8_t reserved_; + uint8_t value[4]; +} CAN_readRegisterResponseMessage; + +typedef struct{ + uint8_t registerAddress; + uint8_t length; + uint8_t reserved_; + uint8_t value[4]; +} CAN_writeRegisterMessage; + +typedef struct{ + uint8_t registerAddress; + uint8_t length; + uint8_t reserved_; + uint8_t value[4]; +} CAN_writeRegisterResponseMessage; + +class CANBusDriver { + public: + CANBusDriver(CONBus& conbus, const uint32_t device_id); + + uint8_t readCanMessage(const uint32_t can_id, const void* buffer); + bool isReplyReady(); + uint8_t peekReply(uint32_t& can_id, uint8_t& can_len, void* buffer); + uint8_t popReply(); + + private: + CONBus& conbus_; + const uint8_t device_id_; + + CAN_readRegisterMessage readRegisterMessage_; + CAN_readRegisterResponseMessage readRegisterResponseMessage_; + CAN_writeRegisterMessage writeRegisterMessage_; + CAN_writeRegisterResponseMessage writeRegisterResponseMessage_; + + bool awaiting_write_response_ = false; + + void putRegisterAddressInQueue(const uint32_t register_address); + uint8_t register_fetch_queue_[256]; + uint8_t register_fetch_queue_head_ = 0; // located at next entry to send + uint8_t register_fetch_queue_tail_ = 0; // located *after* the last entry in the queue +}; + +inline CANBusDriver::CANBusDriver(CONBus& conbus, const uint32_t device_id) : conbus_(conbus), device_id_(device_id) {} + +inline uint8_t CANBusDriver::readCanMessage(const uint32_t can_id, const void* buffer) { + // CONBus read register + if (can_id == ((uint32_t)1000 + device_id_)) { + readRegisterMessage_ = *(CAN_readRegisterMessage*)buffer; + if (readRegisterMessage_.registerAddress != 0xFF) { + putRegisterAddressInQueue(readRegisterMessage_.registerAddress); + } else { + // Put the whole memory map into the queue + for (int i=0; i<255; i++) { + if (conbus_.hasRegister(i)) { + putRegisterAddressInQueue(i); + } + } + } + } + + // CONBus write register + if (can_id == ((uint32_t)1200 + device_id_)) { + awaiting_write_response_ = true; + + writeRegisterMessage_ = *(CAN_writeRegisterMessage*)buffer; + conbus_.writeRegisterBytes(writeRegisterMessage_.registerAddress, writeRegisterMessage_.value, writeRegisterMessage_.length); + + memcpy(&writeRegisterResponseMessage_, &writeRegisterMessage_, sizeof(writeRegisterResponseMessage_)); + } + + return SUCCESS; +} + +inline bool CANBusDriver::isReplyReady() { + return (register_fetch_queue_head_ != register_fetch_queue_tail_) || awaiting_write_response_; +} + +inline uint8_t CANBusDriver::peekReply(uint32_t& can_id, uint8_t& can_len, void* buffer) { + if (register_fetch_queue_head_ != register_fetch_queue_tail_) { + readRegisterResponseMessage_.registerAddress = register_fetch_queue_[register_fetch_queue_head_]; + conbus_.readRegisterBytes(readRegisterResponseMessage_.registerAddress, readRegisterResponseMessage_.value, readRegisterResponseMessage_.length); + + can_id = 1100 + device_id_; + // The readRegisterResponseMessage_ is the full 7 bytes for a 4 byte buffer + // but if we have a smaller message, we should reduce the size + can_len = sizeof(readRegisterResponseMessage_) - (4 - readRegisterResponseMessage_.length); + + memcpy(buffer, &readRegisterResponseMessage_, sizeof(readRegisterResponseMessage_)); + + return SUCCESS; // end early so we dont overwrite a read response with a write response + } + + if (awaiting_write_response_) { + can_id = 1300 + device_id_; + // Same as above, we have to reduce the size appropriately. + can_len = sizeof(writeRegisterResponseMessage_) - (4 - writeRegisterResponseMessage_.length); + memcpy(buffer, &writeRegisterResponseMessage_, sizeof(writeRegisterResponseMessage_)); + } + + return SUCCESS; +} + +inline uint8_t CANBusDriver::popReply() { + if (register_fetch_queue_head_ != register_fetch_queue_tail_) { + // Move head of the queue + register_fetch_queue_head_++; + + return SUCCESS; // end early so we dont overwrite a read response with a write response + } + + if (awaiting_write_response_) { + awaiting_write_response_ = false; + } + + return SUCCESS; +} + +inline void CANBusDriver::putRegisterAddressInQueue(const uint32_t register_address) { + register_fetch_queue_[register_fetch_queue_tail_] = register_address; + register_fetch_queue_tail_++; +} + + +} // end CONBus namespace + +#endif \ No newline at end of file diff --git a/firmware/motor_control/CONBus.h b/firmware/motor_control/CONBus.h new file mode 100644 index 00000000..941b385e --- /dev/null +++ b/firmware/motor_control/CONBus.h @@ -0,0 +1,165 @@ +#ifndef CONBUS_H +#define CONBUS_H + +#include +#include +#include + +namespace CONBus{ + +// Define error codes for all CONBus methods +static const uint8_t SUCCESS = 0; +static const uint8_t ERROR_UNKNOWN = 1; +static const uint8_t ERROR_ADDRESS_ALREADY_USED = 2; +static const uint8_t ERROR_INVALID_LENGTH = 3; +static const uint8_t ERROR_ADDRESS_NOT_REGISTERED = 4; +static const uint8_t ERROR_DIFFERENT_TYPE_THAN_REGISTERED = 5; + +// Define CONBus specification constants +static const uint8_t MAX_REGISTER_LENGTH = 4; + +class CONBus { + public: + CONBus(); + + template + uint8_t addRegister(const uint8_t conbus_address, T* register_address); + template + uint8_t addReadOnlyRegister(const uint8_t conbus_address, T* register_address); + + bool hasRegister(const uint8_t conbus_address); + + template + uint8_t writeRegister(const uint8_t conbus_address, const T value); + uint8_t writeRegisterBytes(const uint8_t conbus_address, const void* buffer, const uint8_t length); + + template + uint8_t readRegister(const uint8_t conbus_address, T& value); + uint8_t readRegisterBytes(const uint8_t conbus_address, void* buffer, uint8_t& length); + private: + uint8_t length_map_[256]; + void* pointer_map_[256]; + bool read_only_map_[256]; +}; + +inline CONBus::CONBus() { + // Clear Length map + for (int i=0; i<256; i++) { + length_map_[i] = 0; + } + + // Clear Pointer map + for (int i=0; i<256; i++) { + pointer_map_[i] = nullptr; + } + + // Clear Read Only map + for (int i=0; i<256; i++) { + read_only_map_[i] = false; + } +} + +template +inline uint8_t CONBus::addRegister(const uint8_t conbus_address, T* register_address) { + if (length_map_[conbus_address] > 0) { + return ERROR_ADDRESS_ALREADY_USED; + } + + if (sizeof(T) > MAX_REGISTER_LENGTH) { + return ERROR_INVALID_LENGTH; + } + + length_map_[conbus_address] = sizeof(T); + pointer_map_[conbus_address] = register_address; + read_only_map_[conbus_address] = false; + + return SUCCESS; +} + +template +inline uint8_t CONBus::addReadOnlyRegister(const uint8_t conbus_address, T* register_address) { + if (length_map_[conbus_address] > 0) { + return ERROR_ADDRESS_ALREADY_USED; + } + + if (sizeof(T) > MAX_REGISTER_LENGTH) { + return ERROR_INVALID_LENGTH; + } + + length_map_[conbus_address] = sizeof(T); + pointer_map_[conbus_address] = register_address; + read_only_map_[conbus_address] = true; + + return SUCCESS; +} + +inline bool CONBus::hasRegister(const uint8_t conbus_address) { + return length_map_[conbus_address] > 0; +} + +template +inline uint8_t CONBus::writeRegister(const uint8_t conbus_address, const T value) { + if (length_map_[conbus_address] == 0) { + return ERROR_ADDRESS_NOT_REGISTERED; + } + + if (sizeof(T) != length_map_[conbus_address]) { + return ERROR_DIFFERENT_TYPE_THAN_REGISTERED; + } + + if (read_only_map_[conbus_address]) { + // If this register is read only, simply don't write. + // TODO: Allow this to be configured to be an ERROR. + return SUCCESS; + } + + *(T*)(pointer_map_[conbus_address]) = value; + + return SUCCESS; +} + +inline uint8_t CONBus::writeRegisterBytes(const uint8_t conbus_address, const void* buffer, const uint8_t length) { + if (length_map_[conbus_address] == 0) { + return ERROR_ADDRESS_NOT_REGISTERED; + } + + if (length != length_map_[conbus_address]) { + return ERROR_DIFFERENT_TYPE_THAN_REGISTERED; + } + + if (read_only_map_[conbus_address]) { + // If this register is read only, simply don't write. + // TODO: Allow this to be configured to be an ERROR. + return SUCCESS; + } + + memcpy(pointer_map_[conbus_address], buffer, length); + + return SUCCESS; +} + +template +inline uint8_t CONBus::readRegister(const uint8_t conbus_address, T& value) { + if (length_map_[conbus_address] == 0) { + return ERROR_ADDRESS_NOT_REGISTERED; + } + + if (sizeof(T) != length_map_[conbus_address]) { + return ERROR_DIFFERENT_TYPE_THAN_REGISTERED; + } + + value = *(T*)(pointer_map_[conbus_address]); + + return SUCCESS; +} + +inline uint8_t CONBus::readRegisterBytes(const uint8_t conbus_address, void* buffer, uint8_t& length) { + memcpy(buffer, pointer_map_[conbus_address], length_map_[conbus_address]); + length = length_map_[conbus_address]; + + return SUCCESS; +} + +} // end CONBus namespace + +#endif \ No newline at end of file diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index 7976e8d1..b8fa5ec9 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -4,8 +4,8 @@ #include "differential_drive.h" #include "motor_with_encoder.h" #include "common.h" -#include -#include +#include "CONBus.h" +#include "CANBusDriver.h" //LED static int LED1 = 22; @@ -34,12 +34,12 @@ static int MCP2515_MISO = 16; static int MCP2515_CS = 17; static int MCP2515_SCK = 18; static int MCP2515_MOSI = 19; -static int MCP215_INT = 20; +static int MCP2515_INT = 20; //Quartz oscillator - 8MHz -static uint23_t QUARTZFREQUENCY = 8UL * 1000UL * 1000UL +static uint32_t QUARTZ_FREQUENCY = 8UL * 1000UL * 1000UL; -ACAN2515 can(MCP2515_CS, SPI0, MCP2515_INT); +ACAN2515 can(MCP2515_CS, SPI, MCP2515_INT); TickTwo motor_update_timer(setMotorUpdateFlag, 25); @@ -87,7 +87,11 @@ float desired_forward_velocity; float desired_angular_velocity; void setup() { - delay(50); + Serial.begin(9600); + while(!Serial){ + delay(50); + } + Serial.println("Serial Connected"); drivetrain.setup(); @@ -100,14 +104,18 @@ void setup() { attachInterrupt(ENC0A, updateRight, CHANGE); motor_update_timer.start(); -} -void setup1(){ - Serial.begin(9600); + + //setup1 delay(50); configureCan(); pinMode(LED1, OUTPUT); pinMode(LED2, OUTPUT); + digitalWrite(LED1, HIGH); + digitalWrite(LED2, HIGH); + delay(1000); + digitalWrite(LED1, LOW); + digitalWrite(LED2, LOW); pinMode(ESTOP, INPUT); conbus.addReadOnlyRegister(0x00, drivetrain.getUpdatePeriod()); @@ -135,7 +143,9 @@ void setup1(){ conbus.addRegister(0x50, &motor_updates_between_deltaodom); } + void loop() { + updateTimers(); if (MOTOR_UPDATE_FLAG) { @@ -144,9 +154,8 @@ void loop() { MOTOR_UPDATE_FLAG = false; } -} -void loop1(){ + //loop1 if (motor_updates_in_deltaodom >= motor_updates_between_deltaodom) { motor_updates_in_deltaodom = 0; sendCanOdomMsgOut(); @@ -162,13 +171,15 @@ void loop1(){ conbus_can.popReply(); } } + } + void configureCan() { - SPI0.setSCK(MCP2515_SCK); - SPI0.setTX(MCP2515_MOSI); - SPI0.setRX(MCP2515_MISO); - SPI0.setCS(MCP2515_CS); - SPI0.begin(); + SPI.setSCK(MCP2515_SCK); + SPI.setTX(MCP2515_MOSI); + SPI.setRX(MCP2515_MISO); + SPI.setCS(MCP2515_CS); + SPI.begin(); Serial.println("Configure ACAN2515"); @@ -176,10 +187,11 @@ void configureCan() { settings.mRequestedMode = ACAN2515Settings::NormalMode ; // Select Normal mode const uint16_t errorCode = can.begin(settings, onCanRecieve ); if (errorCode == 0) { + Serial.println("CAN Configured"); } else{ Serial.print("Error: "); - Serial.print(errorCode); + Serial.println(errorCode); } } void onCanRecieve() { From 7b917e37777153bd7da5e1cc4c14efe0f9b9cf72 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Wed, 31 Jan 2024 17:57:44 -0600 Subject: [PATCH 008/113] Removed print out for frame id and data --- firmware/motor_control/motor_control.ino | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index b8fa5ec9..c1e70a8d 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -205,7 +205,7 @@ void onCanRecieve() { conbus_can.readCanMessage(frame.id, frame.data); - switch (frame.id) { + switch (frame.id) { case 10: motorCommand = *(MotorCommand*)(frame.data); @@ -228,6 +228,7 @@ void onCanRecieve() { drivetrain.setOutput(desired_forward_velocity, desired_angular_velocity); break; } + } PIDSetpoints pid_setpoints; From 624b42bdd2c15abd0519c92dc304f66bc27e05c6 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Tue, 6 Feb 2024 19:56:44 -0600 Subject: [PATCH 009/113] Fixed serial infinite looooooop --- firmware/motor_control/motor_control.ino | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index c1e70a8d..0db70bde 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -88,9 +88,10 @@ float desired_angular_velocity; void setup() { Serial.begin(9600); - while(!Serial){ - delay(50); - } + //while(!Serial){ + //delay(50); + //} + delay(50); Serial.println("Serial Connected"); drivetrain.setup(); @@ -181,7 +182,6 @@ void configureCan() { SPI.setCS(MCP2515_CS); SPI.begin(); - Serial.println("Configure ACAN2515"); ACAN2515Settings settings(QUARTZ_FREQUENCY, 100UL * 1000UL); // CAN bit rate 100 kb/s settings.mRequestedMode = ACAN2515Settings::NormalMode ; // Select Normal mode @@ -205,6 +205,8 @@ void onCanRecieve() { conbus_can.readCanMessage(frame.id, frame.data); + printCanMsg(frame); + switch (frame.id) { case 10: motorCommand = *(MotorCommand*)(frame.data); @@ -212,6 +214,7 @@ void onCanRecieve() { desired_forward_velocity = (float)motorCommand.setpoint_forward_velocity / SPEED_SCALE_FACTOR; desired_angular_velocity = (float)motorCommand.setpoint_angular_velocity / SPEED_SCALE_FACTOR; + if (useObstacleAvoidance && isDetectingObstacle && desired_forward_velocity > 0) { desired_forward_velocity = 0; } From 4aee29bd019426f35926b6cc6d463fdbc2132619 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Wed, 27 Mar 2024 17:57:57 -0500 Subject: [PATCH 010/113] Update wheel radius + base --- firmware/motor_control/conbus_params.h | 111 -------------------- firmware/motor_control/differential_drive.h | 4 +- 2 files changed, 2 insertions(+), 113 deletions(-) delete mode 100644 firmware/motor_control/conbus_params.h diff --git a/firmware/motor_control/conbus_params.h b/firmware/motor_control/conbus_params.h deleted file mode 100644 index 0b6e53dc..00000000 --- a/firmware/motor_control/conbus_params.h +++ /dev/null @@ -1,111 +0,0 @@ -#ifndef CONBUS_PARAMS -#define CONBUS_PARAMS - -#include -#include - -class ConbusParams { - -public: - ConbusParams(int SDA, int SCL, int WP); - - void update_variable(float id, float value); - void refresh(); - - float getUpdatePeriod(); - float getPulsesPerRadian(); - float getWheelRadius(); - float getWheelBaseLength(); - float getSlewRateLimit(); - float getLeftEncoderFactor(); - float getRightEncoderFactor(); - float getVelocitykP(); - float getVelocitykI(); - float getVelocitykD(); - float getVelocitykF(); - float getAngularkP(); - float getAngularkI(); - float getAngularkD(); - float getAngularkF(); - float getUseObstacleAvoidence(); - float getCollisionDist(); - float getSendStatistics(); - float getMotorUpdateBetweenDelatOdom(); - -private: - void refreshVariables(); - void setUpdatePeriod(float period); - void setPulsesPerRadian(float ppr); - void setWheelRadius(float radius); - void setWheelBaseLength(float length); - void setSlewRateLimit(float slewRate); - void setLeftEncoderFactor(float factor); - void setRightEncoderFactor(float factor); - void setVelocitykP(float vkp); - void setVelocitykI(float vki); - void setVelocitykD(float vkd); - void setVelocitykF(float vkf); - void setAngularkP(float akp); - void setAngularkI(float aki); - void setAngularkD(float akd); - void setAngularkF(float akf); - void setUseObstacleAvoidence(bool use); - void setCollisionDist(int distance); - void setSendStatistics(bool send); - void setMotorUpdateBetweenDeltaOdom(bool update); - - float updatePeriod; - float pulsesPerRadian; - float wheelRadius; - float wheelBaseLength; - float slewRateLimit; - float leftEncoderFactor; - float rightEncoderFactor; - float velocitykP; - float velocitykI; - float velocitykD; - float velocitykF; - float angularkP; - float angularkI; - float angularkD; - float angularkF; - bool useObstacleAvoidance; - bool sendStatisics; - bool MotorUpdaatesBetweenDeltaOdom; - int collisionDist; - - int addr_updatePeriod = 0x00; - int addr_pulsesPerRadian = 0x04; - int addr_wheelRadius = 0x08; - int addr_wheelBaseLength = 0xC; - int addr_slewRateLimit = 0x10; - int addr_leftEncoderFactor = 0x14; - int addr_rightEncoderFactor = 0x18; - int addr_velocitykP = 0x1C; - int addr_velocitykI = 0x20; - int addr_velocitykD = 0x24; - int addr_velocitykF = 0x28; - int addr_angularkP = 0x2C; - int addr_angularkI = 0x30; - int addr_angularkD = 0x34; - int addr_angularkF = 0x38; - int addr_useObstacleAvoidance = 0x3C; - int addr_sendStatisics = 0x40; - int addr_MotorUpdaatesBetweenDeltaOdom = 0x44; - int addr_updatePeriod = 0x48; - int addr_collisionDist = 0x4C; - - - int EEPROM_SDA; - int EEPROM_SCL; - int EEPROM_WP; - - -}; - -inline ConbusParams::ConbusParams(int SDA, int SCL, int WP) { - Wire.setSDA(SDA); - Wire.setSCL(SCL); - pinMode(WP, OUTPUT); - -} \ No newline at end of file diff --git a/firmware/motor_control/differential_drive.h b/firmware/motor_control/differential_drive.h index d5a912ab..3c5deb38 100644 --- a/firmware/motor_control/differential_drive.h +++ b/firmware/motor_control/differential_drive.h @@ -45,8 +45,8 @@ class DifferentialDrive { float update_period_ = 0.025f; float pulses_per_radian_ = 600.0f * 20.0f / 16.8f; - float wheel_radius_ = 0.135f; - float wheelbase_length_ = 0.45f; + float wheel_radius_ = 0.19f; + float wheelbase_length_ = 0.575f; float left_encoder_factor_ = 1.00f; float right_encoder_factor_ = 1.01f; From 001bb40bb7b5384d1b4e827540d2af3ac5817423 Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Wed, 27 Mar 2024 18:01:48 -0500 Subject: [PATCH 011/113] update pulses per radian --- firmware/motor_control/differential_drive.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/firmware/motor_control/differential_drive.h b/firmware/motor_control/differential_drive.h index 3c5deb38..c4a0e937 100644 --- a/firmware/motor_control/differential_drive.h +++ b/firmware/motor_control/differential_drive.h @@ -44,7 +44,7 @@ class DifferentialDrive { MotorWithEncoder right_motor_; float update_period_ = 0.025f; - float pulses_per_radian_ = 600.0f * 20.0f / 16.8f; + float pulses_per_radian_ = 600.0f; float wheel_radius_ = 0.19f; float wheelbase_length_ = 0.575f; float left_encoder_factor_ = 1.00f; From 47bc6724f1aecc5de88497c4f9c80f051413665a Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 10 Apr 2024 19:36:09 -0500 Subject: [PATCH 012/113] First iteration of serial dual camera support --- autonav_ws/src/autonav_serial/src/camera.py | 25 ++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 7c2573a3..fcfb6b71 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -20,14 +20,16 @@ def __init__(self): self.refresh_rate = 8 self.output_width = 640 self.output_height = 480 - self.camera_index = 0 + self.camera_index_left = 0 + self.camera_index_right = 1 self.scan_rate = 1.0 class CameraNode(Node): def __init__(self): super().__init__("autonav_serial_camera") - self.camera_publisher = self.create_publisher(CompressedImage, "/autonav/camera/compressed", 20) + self.camera_publisher_left = self.create_publisher(CompressedImage, "/autonav/camera/compressed/left", 20) + self.camera_publisher_right = self.create_publisher(CompressedImage, "/autonav/camera/compressed/right", 20) self.camera_thread = threading.Thread(target=self.camera_worker) self.camera_thread.daemon = True @@ -35,17 +37,27 @@ def init(self): self.get_logger().info("Initializing camera node...") self.camera_thread.start() + def create_threads(self): + self.camera_thread_left = threading.Thread(target=self.camera_worker, args=("left",)) + self.camera_thread_left.daemon = True + + self.camera_thread_right = threading.Thread(target=self.camera_worker, args=("right",)) + self.camera_thread_right.daemon = True + def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) def get_default_config(self): return CameraNodeConfig() - def camera_worker(self): + def camera_worker(self, *args, **kwargs): + index_name = args[0] if len(args) > 0 else "" + camera_index = self.config.camera_index_left if index_name == "left" else self.config.camera_index_right + capture = None while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: try: - if not os.path.exists("/dev/video" + str(self.config.camera_index)): + if not os.path.exists("/dev/video" + str(camera_index)): time.sleep(self.config.scan_rate) continue @@ -79,7 +91,10 @@ def camera_worker(self): if not ret or frame is None: continue - self.camera_publisher.publish(bridge.cv2_to_compressed_imgmsg(frame)) + if index_name == "left": + self.camera_publisher_left.publish(bridge.cv2_to_compressed_imgmsg(frame)) + else: + self.camera_publisher_right.publish(bridge.cv2_to_compressed_imgmsg(frame)) time.sleep(1.0 / self.config.refresh_rate) From 096b011eacd769e0ec7e309441c4e970acba84bb Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 10 Apr 2024 19:55:05 -0500 Subject: [PATCH 013/113] Add autonav_serial camera node to managed_steam.xml --- autonav_ws/src/autonav_launch/launch/managed_steam.xml | 1 + autonav_ws/src/autonav_serial/src/camera.py | 2 +- display/scripts/globals.js | 3 ++- 3 files changed, 4 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 5b1cfed5..d03dcba9 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -14,4 +14,5 @@ + \ No newline at end of file diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index fcfb6b71..20f38d09 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -74,7 +74,7 @@ def camera_worker(self, *args, **kwargs): time.sleep(self.config.scan_rate) continue - while rclpy.ok() and self.getSystemState().state != SystemStateEnum.SHUTDOWN: + while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: if self.device_state != DeviceStateEnum.OPERATING: continue diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 1052e765..a6c7abbb 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -29,7 +29,8 @@ var addressKeys = { "refresh_rate": "int", "output_width": "int", "output_height": "int", - "camera_index": "int" + "camera_index_left": "int", + "camera_index_right": "int" }, "autonav_vision_transformer": { From 7aad66cf9cdc877b7e957b3fa8b53e60c0754536 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 10 Apr 2024 20:05:26 -0500 Subject: [PATCH 014/113] Update camera configuration and image processing in camera.py --- autonav_ws/src/autonav_serial/src/camera.py | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 20f38d09..9dd2b270 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -20,8 +20,8 @@ def __init__(self): self.refresh_rate = 8 self.output_width = 640 self.output_height = 480 - self.camera_index_left = 0 - self.camera_index_right = 1 + self.camera_index_left = 1 + self.camera_index_right = 2 self.scan_rate = 1.0 @@ -80,6 +80,9 @@ def camera_worker(self, *args, **kwargs): try: ret, frame = capture.read() + frame = cv2.flip(frame, 1) + frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) + frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) except: if capture is not None: capture.release() From 0e1dcc3e44bf7ecc51717cf889c75846db98a71e Mon Sep 17 00:00:00 2001 From: BrandonKahn25 Date: Fri, 12 Apr 2024 10:51:00 -0500 Subject: [PATCH 015/113] Swapped pins for encoders --- firmware/motor_control/motor_control.ino | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/firmware/motor_control/motor_control.ino b/firmware/motor_control/motor_control.ino index 0db70bde..fab68d40 100644 --- a/firmware/motor_control/motor_control.ino +++ b/firmware/motor_control/motor_control.ino @@ -19,10 +19,10 @@ static int PWM0 = 6; //right motor controller static int PWM1 = 7; //left motor controller //ENCODER -static int ENC0A = 8; //right encoder -static int ENC0B = 9; //right encoder -static int ENC1A = 10; //left encoder -static int ENC1B = 11; //left encoder +static int ENC0A = 9; //right encoder +static int ENC0B = 8; //right encoder +static int ENC1A = 11; //left encoder +static int ENC1B = 10; //left encoder //EEPROM static int EEPROMSDA = 0; From 9ae0b8dcf01b16ff3e05f3d560bee1b8ff383b26 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Fri, 12 Apr 2024 14:35:39 -0500 Subject: [PATCH 016/113] Added vectornav to manual launch --- autonav_ws/src/autonav_launch/launch/managed_steam.xml | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index fa972043..ec4fb81c 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -1,8 +1,8 @@ - - + + @@ -24,6 +24,7 @@ + From a43d1f6c82b67d3306c5229137dda93a36e364c5 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Fri, 12 Apr 2024 14:52:11 -0500 Subject: [PATCH 017/113] Fixed dual camera serial node --- autonav_ws/src/autonav_serial/src/camera.py | 23 ++++++++++--------- .../src/autonav_serial/src/serial_node.py | 4 ++++ 2 files changed, 16 insertions(+), 11 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 9dd2b270..a6bbe8d9 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -20,7 +20,7 @@ def __init__(self): self.refresh_rate = 8 self.output_width = 640 self.output_height = 480 - self.camera_index_left = 1 + self.camera_index_left = 0 self.camera_index_right = 2 self.scan_rate = 1.0 @@ -30,19 +30,19 @@ def __init__(self): super().__init__("autonav_serial_camera") self.camera_publisher_left = self.create_publisher(CompressedImage, "/autonav/camera/compressed/left", 20) self.camera_publisher_right = self.create_publisher(CompressedImage, "/autonav/camera/compressed/right", 20) - self.camera_thread = threading.Thread(target=self.camera_worker) - self.camera_thread.daemon = True def init(self): self.get_logger().info("Initializing camera node...") - self.camera_thread.start() + self.create_threads() def create_threads(self): self.camera_thread_left = threading.Thread(target=self.camera_worker, args=("left",)) self.camera_thread_left.daemon = True + self.camera_thread_left.start() self.camera_thread_right = threading.Thread(target=self.camera_worker, args=("right",)) self.camera_thread_right.daemon = True + self.camera_thread_right.start() def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) @@ -61,26 +61,27 @@ def camera_worker(self, *args, **kwargs): time.sleep(self.config.scan_rate) continue - capture = cv2.VideoCapture(0) + capture = cv2.VideoCapture(camera_index) if capture is None or not capture.isOpened(): time.sleep(self.config.scan_rate) continue capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.config.output_width) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.config.output_height) - self.set_device_state(DeviceStateEnum.OPERATING) + # self.set_device_state(DeviceStateEnum.OPERATING) except: - self.set_device_state(DeviceStateEnum.STANDBY) + # self.set_device_state(DeviceStateEnum.STANDBY) time.sleep(self.config.scan_rate) continue while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: - if self.device_state != DeviceStateEnum.OPERATING: - continue + # if self.device_state != DeviceStateEnum.OPERATING: + # continue try: ret, frame = capture.read() - frame = cv2.flip(frame, 1) + if index_name == "left": + frame = cv2.flip(frame, 1) frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) except: @@ -88,7 +89,7 @@ def camera_worker(self, *args, **kwargs): capture.release() capture = None - self.set_device_state(DeviceStateEnum.STANDBY) + # self.set_device_state(DeviceStateEnum.STANDBY) break if not ret or frame is None: diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index 56a4325d..4c4973d6 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -2,6 +2,7 @@ from ctypes import Structure, c_bool, c_uint8 import rclpy +import time import can import threading import struct @@ -72,6 +73,9 @@ def canThreadWorker(self): except can.CanError: pass + def getClockMs(self): + return time.time() * 1000.0 + def onCanMessageReceived(self, msg): arb_id = msg.arbitration_id if arb_id == MOTOR_FEEDBACK_ID: From 6a67d98e7a8417a28ba68946c0ffa5e23acd33a9 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 16 Apr 2024 18:25:32 -0500 Subject: [PATCH 018/113] Add blink_period parameter to SafetyLightsPacket structure and set default value in SafetyLightsSerial class --- autonav_ws/src/autonav_serial/src/safety_lights.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_serial/src/safety_lights.py b/autonav_ws/src/autonav_serial/src/safety_lights.py index 4bde4d25..30bf6b97 100644 --- a/autonav_ws/src/autonav_serial/src/safety_lights.py +++ b/autonav_ws/src/autonav_serial/src/safety_lights.py @@ -20,7 +20,8 @@ class SafetyLightsPacket(Structure): ("brightness", c_uint8, 8), ("red", c_uint8, 8), ("green", c_uint8, 8), - ("blue", c_uint8, 8) + ("blue", c_uint8, 8), + ("blink_period", c_uint8, 8) ] @@ -70,6 +71,8 @@ def onSafetyLightsReceived(self, lights: SafetyLights): data["red"] = lights.red data["green"] = lights.green data["blue"] = lights.blue + data["blink_period"] = 500 + self.writeQueueLock.acquire() self.writeQueue.append(data) self.writeQueueLock.release() From 2c91dab2b235d2e878eda01c281fb7ad991e6ff0 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 16 Apr 2024 18:28:10 -0500 Subject: [PATCH 019/113] Add blink_period parameter to SafetyLightsPacket structure and set default value in SafetyLightsSerial class --- autonav_ws/src/autonav_serial/src/serial_node.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index 4c4973d6..7cf8a3d2 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -31,7 +31,8 @@ class SafetyLightsPacket(Structure): ("brightness", c_uint8, 8), ("red", c_uint8, 8), ("green", c_uint8, 8), - ("blue", c_uint8, 8) + ("blue", c_uint8, 8), + ("blink_period", c_uint8, 8) ] @@ -165,6 +166,7 @@ def onSafetyLightsReceived(self, lights: SafetyLights): packed_data.red = lights.red packed_data.green = lights.green packed_data.blue = lights.blue + packed_data.blink_period = 500 can_msg = can.Message( arbitration_id=SAFETY_LIGHTS_ID, data=bytes(packed_data)) try: From c8b969774b48d5940ac54484807e29a3c8af1c40 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 16 Apr 2024 18:31:11 -0500 Subject: [PATCH 020/113] Fix safety lights behavior when switching to autonomous mode in SteamTranslationNode --- autonav_ws/src/autonav_manual/src/steam.py | 1 + 1 file changed, 1 insertion(+) diff --git a/autonav_ws/src/autonav_manual/src/steam.py b/autonav_ws/src/autonav_manual/src/steam.py index 38a7e5e5..47e7cc52 100644 --- a/autonav_ws/src/autonav_manual/src/steam.py +++ b/autonav_ws/src/autonav_manual/src/steam.py @@ -90,6 +90,7 @@ def onButtonReleased(self, button: SteamControllerButton, msTime: float): self.safetyLightsPublisher.publish(toSafetyLights(False, False, 2, 100, "#FF6F00")) if button == SteamControllerButton.STEAM and self.system_state != SystemStateEnum.AUTONOMOUS: + self.safetyLightsPublisher.publish(toSafetyLights(True, False, 2, 100, "#FF0000")) self.set_system_state(SystemStateEnum.AUTONOMOUS) if button == SteamControllerButton.BACK and self.system_state != SystemStateEnum.DISABLED: From da36c25b0d9d2e86bb70e1cdb9d77564505be9d4 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 23 Apr 2024 09:46:31 -0500 Subject: [PATCH 021/113] Update setup files to use 2024 repo --- setup/{ => etc}/autonav.rules | 0 setup/{ => etc}/autonav.service | 2 +- setup/{ => etc}/autonav_service.sh | 4 ++-- setup/{ => etc}/steam.sh | 0 setup/{ => etc}/vnav.sh | 0 setup/jams.sh | 5 ----- setup/setup.sh | 12 +++--------- 7 files changed, 6 insertions(+), 17 deletions(-) rename setup/{ => etc}/autonav.rules (100%) rename setup/{ => etc}/autonav.service (87%) rename setup/{ => etc}/autonav_service.sh (80%) rename setup/{ => etc}/steam.sh (100%) rename setup/{ => etc}/vnav.sh (100%) delete mode 100755 setup/jams.sh diff --git a/setup/autonav.rules b/setup/etc/autonav.rules similarity index 100% rename from setup/autonav.rules rename to setup/etc/autonav.rules diff --git a/setup/autonav.service b/setup/etc/autonav.service similarity index 87% rename from setup/autonav.service rename to setup/etc/autonav.service index 87d6432d..2bb7edc3 100644 --- a/setup/autonav.service +++ b/setup/etc/autonav.service @@ -1,5 +1,5 @@ [Unit] -Description=Autonav 2023 Service +Description=Autonav 2024 Service [Service] Type=simple diff --git a/setup/autonav_service.sh b/setup/etc/autonav_service.sh similarity index 80% rename from setup/autonav_service.sh rename to setup/etc/autonav_service.sh index d123c582..66f7c22d 100644 --- a/setup/autonav_service.sh +++ b/setup/etc/autonav_service.sh @@ -6,7 +6,7 @@ export DISPLAY=:0 source /opt/ros/humble/setup.bash # Check if build is needed, if so, build -cd /home/$LOCAL_USER/autonav_software_2023/autonav_ws +cd /home/$LOCAL_USER/autonav_software_2024/autonav_ws UPSTREAM=${1:-'@{u}'} LOCAL=$(git rev-parse @) REMOTE=$(git rev-parse "$UPSTREAM") @@ -21,5 +21,5 @@ else fi # Launch -source /home/$LOCAL_USER/autonav_software_2023/autonav_ws/install/setup.bash +source /home/$LOCAL_USER/autonav_software_2024/autonav_ws/install/setup.bash ros2 launch autonav_launch competition.xml \ No newline at end of file diff --git a/setup/steam.sh b/setup/etc/steam.sh similarity index 100% rename from setup/steam.sh rename to setup/etc/steam.sh diff --git a/setup/vnav.sh b/setup/etc/vnav.sh similarity index 100% rename from setup/vnav.sh rename to setup/etc/vnav.sh diff --git a/setup/jams.sh b/setup/jams.sh deleted file mode 100755 index 36a4a2d2..00000000 --- a/setup/jams.sh +++ /dev/null @@ -1,5 +0,0 @@ -# Download and unzip the best jams around -curl --output jams.zip --netrc-file vectorsecrets.txt https://files.dylanzeml.in/protected/jams.zip -mkdir -p $HOME/autonav_software_2023/deps/songs -unzip -o jams.zip -d $HOME/autonav_software_2023/deps/songs -rm jams.zip \ No newline at end of file diff --git a/setup/setup.sh b/setup/setup.sh index 4290c79b..50a1da4d 100755 --- a/setup/setup.sh +++ b/setup/setup.sh @@ -19,19 +19,13 @@ bash vnav.sh # Steam Controller Dependencies bash steam.sh -# Music Dependencies -bash jams.sh - -# ImGUI Dependencies -sudo apt install libglfw3-dev libglew-dev -y - # Python deps sudo apt install python3-pip -y pip3 install python-can[serial] pip3 install websockets # Copy the udev rules to the correct location -sudo cp autonav.rules /etc/udev/rules.d/autonav.rules +sudo cp etc/autonav.rules /etc/udev/rules.d/autonav.rules # Reload udev sudo service udev reload @@ -39,8 +33,8 @@ sleep 2 sudo service udev restart # Copy services -sudo cp autonav.service /etc/systemd/system/autonav.service -sudo cp autonav_service.sh /usr/bin/autonav_service.sh +sudo cp etc/autonav.service /etc/systemd/system/autonav.service +sudo cp etc/autonav_service.sh /usr/bin/autonav_service.sh # chmod time :D sudo chmod +x /usr/bin/autonav_service.sh From 0f8905626aa7cf91b7f5a6fb993264bde5085db7 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Fri, 26 Apr 2024 14:50:19 -0500 Subject: [PATCH 022/113] Added new config options to combiner --- .../src/autonav_vision/src/combination.py | 9 ++-- .../src/autonav_vision/src/transformations.py | 2 - display/scripts/globals.js | 7 +++- display/scripts/main.js | 41 +++++++++++++++++++ 4 files changed, 52 insertions(+), 7 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 527f256a..a503c33a 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -31,9 +31,11 @@ class ImageCombinerConfig: def __init__(self): - self.overlap = 0 self.map_res = 80 - + self.top_left = [147, 287] + self.top_right = [851, 232] + self.bottom_right = [955, 678] + self.bottom_left = [0, 678] class ImageCombiner(Node): def __init__(self): @@ -119,7 +121,8 @@ def try_combine_images(self): cv2img_left = g_bridge.compressed_imgmsg_to_cv2(self.image_left) cv2img_right = g_bridge.compressed_imgmsg_to_cv2(self.image_right) combined = cv2.hconcat([cv2img_left, cv2img_right]) - pts = [(147, 287), (851, 232), (955, 678), (0, 678)] + # ordered as: top-left, top-right, bottom-right, bottom-left + pts = [self.config.top_left, self.config.top_right, self.config.bottom_right, self.config.bottom_left] combined = self.four_point_transform(combined, np.array(pts)) datamap = cv2.resize(combined, dsize=(self.config.map_res, self.config.map_res), interpolation=cv2.INTER_LINEAR) / 2 flat = list(datamap.flatten().astype(int)) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 19cf8d77..6a0ebc13 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -37,8 +37,6 @@ def __init__(self): self.blur_iterations = 3 self.rod_offset = 130 self.map_res = 80 - self.image_warp_tl = 0.26 - self.image_warp_tr = 0.26 class ImageTransformer(Node): diff --git a/display/scripts/globals.js b/display/scripts/globals.js index efede717..75753344 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -107,8 +107,11 @@ var addressKeys = { "autonav_image_combiner": { "internal_title": "[Image Combiner]", - "overlap": "int", - "map_res": "int" + "map_res": "int", + "top_left": "point.int", + "top_right": "point.int", + "bottom_right": "point.int", + "bottom_left": "point.int" } } diff --git a/display/scripts/main.js b/display/scripts/main.js index d16495e7..90f6b4b1 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -644,6 +644,47 @@ $(document).ready(function () { div.appendChild(input); return div; } + else if (type == "point.int") { + // x, y point for two integers + const div = document.createElement("div"); + div.classList.add("input-group"); + div.classList.add("mb-3"); + + const inputX = document.createElement("input"); + inputX.type = "number"; + inputX.classList.add("form-control"); + inputX.value = data[0]; + inputX.onchange = function () { + config[device][text][0] = parseInt(inputX.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputY = document.createElement("input"); + inputY.type = "number"; + inputY.classList.add("form-control"); + inputY.value = data[1]; + inputY.onchange = function () { + config[device][text][1] = parseInt(inputY.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const span = document.createElement("span"); + span.classList.add("input-group-text"); + span.innerText = text; + + div.appendChild(span); + div.appendChild(inputX); + div.appendChild(inputY); + return div; + } else { const options = addressKeys[device][text]; From 5cc8416db919bbe238304360d4dce9da328511e3 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 30 Apr 2024 10:36:38 -0500 Subject: [PATCH 023/113] Fix general system info not updating on display load --- autonav_ws/src/autonav_display/src/display.py | 23 +++++++++++-------- display/index.html | 8 ------- display/scripts/main.js | 17 ++++++-------- display/scripts/utilities.js | 9 ++++++++ 4 files changed, 30 insertions(+), 27 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 5ede35a5..27e0a77e 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -161,20 +161,25 @@ async def consumer(self, websocket): "op": "get_nodes_callback", "nodes": nodes, "states": node_states, - "configs": self.node_configs + "configs": self.node_configs, + "system": { + "state": self.system_state, + "mode": self.system_mode, + "mobility": self.mobility + } }), unique_id) if obj["op"] == "set_system_state": self.set_system_total_state(int(obj["state"]), self.system_mode, bool(obj["mobility"])) - # if obj["op"] == "conbus": - # id = int(obj["id"]) - # data = list(map(lambda x: int(x), obj["data"])) - # msg = Conbus() - # msg.id = id - # msg.data = data - # msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0 - # self.conbusPublisher.publish(msg) + if obj["op"] == "conbus": + id = int(obj["id"]) + data = list(map(lambda x: int(x), obj["data"])) + msg = Conbus() + msg.id = id + msg.data = data + msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0 + self.conbusPublisher.publish(msg) async def handler(self, websocket): unique_id = self.getUserIdFromSocket(websocket) diff --git a/display/index.html b/display/index.html index dc9a5ceb..22529ada 100644 --- a/display/index.html +++ b/display/index.html @@ -56,7 +56,6 @@

General

State:
Mode:
Mobility:
-
ESTOP:
@@ -212,13 +211,6 @@

System

  • Shutdown
    • - -
    - - -
    diff --git a/display/scripts/main.js b/display/scripts/main.js index 90f6b4b1..b8fad84a 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -94,6 +94,12 @@ $(document).ready(function () { config[key] = obj.configs[key]; } regenerateConfig(); + + // Update system state + let system = obj["system"]; + $("#var_system_state").text(system["state"] == 0 ? "Diabled" : system["state"] == 1 ? "Autonomous" : system["state"] == 2 ? "Manual" : "Shutdown"); + $("#var_system_mode").text(system["mode"] == 0 ? "Competition" : system["mode"] == 1 ? "Simulation" : "Practice"); + $("#var_system_mobility").text(system["mobility"] ? "Enabled" : "Disabled"); } } }; @@ -124,7 +130,6 @@ $(document).ready(function () { send({ op: "set_system_state", state: systemState.state, - estop: systemState.estop, mobility: systemState.mobility, }); } @@ -285,22 +290,19 @@ $(document).ready(function () { } if (topic == "/scr/state/system") { - const { state, mode, mobility, estop } = msg; + const { state, mode, mobility } = msg; $("#var_system_state").text(state == 0 ? "Diabled" : state == 1 ? "Autonomous" : state == 2 ? "Manual" : "Shutdown"); $("#var_system_mode").text(mode == 0 ? "Competition" : mode == 1 ? "Simulation" : "Practice"); $("#var_system_mobility").text(mobility ? "Enabled" : "Disabled"); - $("#var_system_estop").text(estop ? "Yes" : "No"); systemState.state = state; systemState.mode = mode; systemState.mobility = mobility; - systemState.estop = estop; $("#input_system_state").val(state); $("#input_system_mode").val(mode); $("#input_system_mobility").prop("checked", mobility); - $("#input_system_estop").prop("checked", estop); return; } @@ -519,11 +521,6 @@ $(document).ready(function () { } }); - $("#checkbox_system_estop").on("change", function () { - systemState.estop = $(this).is(":checked"); - setSystemState(); - }); - $("#checkbox_system_mobility").on("change", function () { systemState.mobility = $(this).is(":checked"); setSystemState(); diff --git a/display/scripts/utilities.js b/display/scripts/utilities.js index c529122d..ad9fef1c 100644 --- a/display/scripts/utilities.js +++ b/display/scripts/utilities.js @@ -28,6 +28,15 @@ const transferImageToElement = (id, data) => { img.src = "data:image/jpeg;base64," + data; } +const trasferImageBytesToElement = (id, data) => { + const img = document.getElementById(id); + const msgarray = new Uint8Array(data); + const blob = new Blob([msgarray], { type: "image/jpeg" }); + const urlCreator = window.URL || window.webkitURL; + const imageUrl = urlCreator.createObjectURL(blob); + img.src = imageUrl; +} + const fromFloatToBytes = (value) => { var buffer = new ArrayBuffer(4); var view = new DataView(buffer); From d77af84cd1d5601f1d9915e83c08013655cf27b4 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 30 Apr 2024 11:06:45 -0500 Subject: [PATCH 024/113] Fix expandify config not working and use astar display --- autonav_ws/src/autonav_display/src/display.py | 2 +- .../src/autonav_launch/launch/simulation.xml | 5 +++ .../src/autonav_vision/src/expandify.cpp | 45 ++++++++++++++----- display/index.html | 7 +++ display/scripts/globals.js | 5 ++- 5 files changed, 52 insertions(+), 12 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 27e0a77e..0998fdcf 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -82,7 +82,7 @@ def __init__(self): self.motorInputSubscriber = self.create_subscription(MotorInput, "/autonav/MotorInput", self.motorInputCallback, 20) self.motorControllerDebugSubscriber = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.motorControllerDebugCallback, 20) self.objectDetectionSubscriber = self.create_subscription(ObjectDetection, "/autonav/ObjectDetection", self.objectDetectionCallback, 20) - self.pathingDebugSubscriber = self.create_subscription(PathingDebug, "/autonav/debug/astar", self.pathingDebugCallback, 20) + self.pathingDebugSubscriber = self.create_subscription(PathingDebug, "/autonav/cfg_space/expanded/image", self.pathingDebugCallback, 20) self.gpsFeedbackSubscriber = self.create_subscription(GPSFeedback, "/autonav/gps", self.gpsFeedbackCallback, 20) self.imuDataSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.imuDataCallback, 20) self.conbusSubscriber = self.create_subscription(Conbus, "/autonav/conbus/data", self.conbusCallback, 100) diff --git a/autonav_ws/src/autonav_launch/launch/simulation.xml b/autonav_ws/src/autonav_launch/launch/simulation.xml index 1de7695f..dfe15047 100644 --- a/autonav_ws/src/autonav_launch/launch/simulation.xml +++ b/autonav_ws/src/autonav_launch/launch/simulation.xml @@ -18,6 +18,11 @@ + + + + + diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 89456876..9b98dbe5 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -11,8 +11,11 @@ struct ExpandifyConfig float vertical_fov; float horizontal_fov; float map_res; + float max_range; + float no_go_percent; + float no_go_range; - NLOHMANN_DEFINE_TYPE_INTRUSIVE(ExpandifyConfig, vertical_fov, horizontal_fov, map_res) + NLOHMANN_DEFINE_TYPE_INTRUSIVE(ExpandifyConfig, vertical_fov, horizontal_fov, map_res, max_range, no_go_percent, no_go_range) }; struct Circle @@ -38,32 +41,51 @@ class ExpandifyNode : public SCR::Node map.origin.position.x = -10.0; map.origin.position.y = -10.0; + build_circles(); + + raw_map_subscriber = create_subscription("/autonav/cfg_space/raw", 20, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); + expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", 20); + debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", 20); + + set_device_state(SCR::DeviceState::OPERATING); + } + + void build_circles() + { + // Reset variables + maxRange = config.max_range; + noGoPercent = config.no_go_percent; + noGoRange = config.no_go_range; + + // Calculate the range of the circles auto tempRange = maxRange * noGoPercent; maxRange = (int)(maxRange / (config.horizontal_fov / config.map_res)); noGoRange = (int)(tempRange / (config.horizontal_fov / config.map_res)); - circles.push_back(Circle{0, 0, 0}); + // Build the circles + std::vector newCircles; + newCircles.push_back(Circle{0, 0, 0}); for (int x = -maxRange; x <= maxRange; x++) { for (int y = -maxRange; y <= maxRange; y++) { + // Check if the point is within the circle if (maxRange * noGoPercent <= sqrt(x * x + y * y) && sqrt(x * x + y * y) < maxRange) { - circles.push_back(Circle{x, y, sqrt(x * x + y * y)}); + // Add the point to the circle + newCircles.push_back(Circle{x, y, sqrt(x * x + y * y)}); } } } - raw_map_subscriber = create_subscription("/autonav/cfg_space/raw", 20, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); - expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", 20); - debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", 20); - - set_device_state(SCR::DeviceState::OPERATING); + circles = newCircles; } void config_updated(json newConfig) override { config = newConfig.template get(); + RCLCPP_INFO(this->get_logger(), "Config updated: vertical_fov: %f, horizontal_fov: %f, map_res: %f", config.vertical_fov, config.horizontal_fov, config.map_res); + build_circles(); } json get_default_config() override @@ -72,6 +94,9 @@ class ExpandifyNode : public SCR::Node newConfig.vertical_fov = 2.75; newConfig.horizontal_fov = 3; newConfig.map_res = 80.0f; + newConfig.max_range = 0.65; + newConfig.no_go_percent = 0.70; + newConfig.no_go_range = 0; return newConfig; } @@ -151,8 +176,8 @@ class ExpandifyNode : public SCR::Node nav_msgs::msg::MapMetaData map; - float maxRange = 0.10; - float noGoPercent = 0.38; + float maxRange = 0.65; + float noGoPercent = 0.70; int noGoRange = 0; std::vector circles; ExpandifyConfig config; diff --git a/display/index.html b/display/index.html index 22529ada..0e27006c 100644 --- a/display/index.html +++ b/display/index.html @@ -211,6 +211,13 @@

    System

  • Shutdown
    • + +
    + + +
    diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 75753344..5fde2813 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -51,7 +51,10 @@ var addressKeys = { "internal_title": "[Vision] Expandifier", "horizontal_fov": "int", "map_res": "int", - "vertical_fov": "float" + "vertical_fov": "float", + "max_range": "float", + "no_go_percent": "float", + "no_go_range": "float", }, "autonav_filters": { From 6036b1860a0399f099fe99664a75a8837bacf7d1 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 30 Apr 2024 18:47:27 -0500 Subject: [PATCH 025/113] god help us all --- autonav_ws/src/autonav_display/src/display.py | 12 ++++++------ autonav_ws/src/autonav_nav/src/astar.py | 14 +++++++------- autonav_ws/src/autonav_nav/src/path_resolver.py | 9 +++++---- autonav_ws/src/autonav_vision/src/combination.py | 8 ++++---- autonav_ws/src/autonav_vision/src/expandify.cpp | 6 +++--- .../src/autonav_vision/src/transformations.py | 7 ++++--- autonav_ws/src/scr/include/scr/node.hpp | 3 +++ autonav_ws/src/scr/scr/node.py | 6 ++++++ autonav_ws/src/scr/src/node.cpp | 3 +++ 9 files changed, 41 insertions(+), 27 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 0998fdcf..81630b1f 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -90,12 +90,12 @@ def __init__(self): self.systemStateService = self.create_client(SetSystemState, "/scr/state/set_system_state") - self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.cameraCallbackLeft, 20) - self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.cameraCallbackRight, 20) - self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, 20) - self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, 20) - self.bigboiSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.bigboiCallback, 20) - self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/expanded/image", self.debugAStarCallback, 20) + self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.cameraCallbackLeft, self.qos_profile) + self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.cameraCallbackRight, self.qos_profile) + self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, self.qos_profile) + self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, self.qos_profile) + self.bigboiSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.bigboiCallback, self.qos_profile) + self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, self.qos_profile) self.get_logger().info("Starting event loop") diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index f64da18e..6730a049 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -90,13 +90,13 @@ def get_default_config(self): return AStarNodeConfig() def init(self): - self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.onConfigSpaceReceived, 20) - self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.onPoseReceived, 20) - self.imuSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.onImuReceived, 20) - self.debugPublisher = self.create_publisher(PathingDebug, "/autonav/debug/astar", 20) - self.pathPublisher = self.create_publisher(Path, "/autonav/path", 20) - self.safetyLightsPublisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 20) - self.pathDebugImagePublisher = self.create_publisher(CompressedImage, "/autonav/debug/astar/image", 20) + self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.onConfigSpaceReceived, self.qos_profile) + self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.onPoseReceived, self.qos_profile) + self.imuSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.onImuReceived, self.qos_profile) + self.debugPublisher = self.create_publisher(PathingDebug, "/autonav/debug/astar", self.qos_profile) + self.pathPublisher = self.create_publisher(Path, "/autonav/path", self.qos_profile) + self.safetyLightsPublisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", self.qos_profile) + self.pathDebugImagePublisher = self.create_publisher(CompressedImage, "/autonav/debug/astar/image", self.qos_profile) self.mapTimer = self.create_timer(0.1, self.createPath) self.resetWhen = -1.0 diff --git a/autonav_ws/src/autonav_nav/src/path_resolver.py b/autonav_ws/src/autonav_nav/src/path_resolver.py index 828fc4dd..16778c8a 100644 --- a/autonav_ws/src/autonav_nav/src/path_resolver.py +++ b/autonav_ws/src/autonav_nav/src/path_resolver.py @@ -3,6 +3,7 @@ import json from types import SimpleNamespace from autonav_msgs.msg import MotorInput, Position, SafetyLights +import rclpy.qos from scr_msgs.msg import SystemState from scr.node import Node from scr.states import DeviceStateEnum, SystemStateEnum @@ -56,10 +57,10 @@ def init(self): self.pure_pursuit = PurePursuit() self.backCount = -1 self.status = -1 - self.path_subscriber = self.create_subscription(Path, "/autonav/path", self.on_path_received, 20) - self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.on_position_received, 20) - self.motor_publisher = self.create_publisher(MotorInput, "/autonav/MotorInput", 20) - self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 20) + self.path_subscriber = self.create_subscription(Path, "/autonav/path", self.on_path_received, self.qos_profile) + self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.on_position_received, self.qos_profile) + self.motor_publisher = self.create_publisher(MotorInput, "/autonav/MotorInput", 1) + self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", self.qos_profile) self.config = self.get_default_config() self.create_timer(0.05, self.onResolve) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index a503c33a..4ad9448a 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -44,10 +44,10 @@ def __init__(self): def init(self): self.image_left = None self.image_right = None - self.image_left_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.image_received_left, 10) - self.image_right_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.image_received_right, 10) - self.combined_image_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/raw", 10) - self.combined_image_publisher_debug = self.create_publisher(CompressedImage, "/autonav/cfg_space/raw/debug", 10) + self.image_left_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.image_received_left, self.qos_profile) + self.image_right_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.image_received_right, self.qos_profile) + self.combined_image_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/raw", self.qos_profile) + self.combined_image_publisher_debug = self.create_publisher(CompressedImage, "/autonav/cfg_space/raw/debug", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) def image_received_left(self, msg): diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 9b98dbe5..096c78c5 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -43,9 +43,9 @@ class ExpandifyNode : public SCR::Node build_circles(); - raw_map_subscriber = create_subscription("/autonav/cfg_space/raw", 20, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); - expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", 20); - debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", 20); + raw_map_subscriber = create_subscription("/autonav/cfg_space/raw", qos_profile, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); + expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", qos_profile); + debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", qos_profile); set_device_state(SCR::DeviceState::OPERATING); } diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 6a0ebc13..0c5186bb 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -6,6 +6,7 @@ import numpy as np from nav_msgs.msg import MapMetaData, OccupancyGrid +import rclpy.qos from sensor_msgs.msg import CompressedImage from geometry_msgs.msg import Pose from cv_bridge import CvBridge @@ -48,9 +49,9 @@ def directionify(self, topic): return topic + "/" + self.dir def init(self): - self.cameraSubscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, 1) - self.rawMapPublisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/preraw"), 1) - self.filteredImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image"), 1) + self.cameraSubscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, self.qos_profile) + self.rawMapPublisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/preraw"), self.qos_profile) + self.filteredImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image"), self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) diff --git a/autonav_ws/src/scr/include/scr/node.hpp b/autonav_ws/src/scr/include/scr/node.hpp index a041e2d9..3d3cb4ed 100644 --- a/autonav_ws/src/scr/include/scr/node.hpp +++ b/autonav_ws/src/scr/include/scr/node.hpp @@ -133,6 +133,9 @@ namespace SCR /// @brief The current map of device configurations std::map device_states; + /// @brief The current qos profile + rclcpp::QoS qos_profile = rclcpp::QoS(rclcpp::KeepLast(10)); + private: NodeSubscriptions subscriptions; NodePublishers publishers; diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index 71879696..2b64d248 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -5,6 +5,7 @@ from std_msgs.msg import Float64 from rclpy.node import Node as ROSNode from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy import scr.constants import time import rclpy @@ -20,6 +21,11 @@ class Node(ROSNode): def __init__(self, node_name): super().__init__(node_name) self.identifier = node_name + self.qos_profile = QoSProfile( + reliability = QoSReliabilityPolicy.BEST_EFFORT, + history = QoSHistoryPolicy.KEEP_LAST, + depth = 1 + ) # Create the callback groups self.device_state_callback_group = MutuallyExclusiveCallbackGroup() diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index cd033467..dfab8e05 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -26,6 +26,9 @@ namespace SCR clients.device_state = this->create_client(Constants::Services::DEVICE_STATE, rmw_qos_profile_services_default, callback_groups.device_state); clients.config_update = this->create_client(Constants::Services::CONFIG_UPDATE, rmw_qos_profile_services_default, callback_groups.config_updated); + rmw_qos_profile_t q = rmw_qos_profile_sensor_data; + qos_profile = rclcpp::QoS(rclcpp::QoSInitialization(q.history, 1), q); + // Create a thread to wait a sec for the node to boot without blocking the main thread std::thread booting_thread([this]() { From f97153c961ab8c2c4ad2917898ca3bcb0d60bdd1 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 30 Apr 2024 19:27:52 -0500 Subject: [PATCH 026/113] Add expandify node to competition.xml --- autonav_ws/src/autonav_launch/launch/competition.xml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/autonav_ws/src/autonav_launch/launch/competition.xml b/autonav_ws/src/autonav_launch/launch/competition.xml index 662946c6..14e3e0d1 100644 --- a/autonav_ws/src/autonav_launch/launch/competition.xml +++ b/autonav_ws/src/autonav_launch/launch/competition.xml @@ -17,6 +17,7 @@ + @@ -28,4 +29,8 @@ + + + +     \ No newline at end of file From 68c8c06b96da133a3b804ae247622a4bb5b12e79 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 30 Apr 2024 19:44:18 -0500 Subject: [PATCH 027/113] Fix camera flip issue in autonav_serial/src/camera.py --- autonav_ws/src/autonav_serial/src/camera.py | 1 + 1 file changed, 1 insertion(+) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index a6bbe8d9..3987f423 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -82,6 +82,7 @@ def camera_worker(self, *args, **kwargs): ret, frame = capture.read() if index_name == "left": frame = cv2.flip(frame, 1) + frame = cv2.flip(frame, 0) frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) except: From 7ed68f64cbc132530fc501442c0f40a10cc30b6d Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 1 May 2024 15:57:55 -0500 Subject: [PATCH 028/113] First iteration of updated vision --- .../src/autonav_vision/src/combination.py | 130 ++++-------------- .../src/autonav_vision/src/expandify.cpp | 2 +- .../src/autonav_vision/src/transformations.py | 128 +++++++++++++---- 3 files changed, 133 insertions(+), 127 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 4ad9448a..49fef4ab 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -3,15 +3,12 @@ from types import SimpleNamespace import rclpy import json -import cv2 -import numpy as np from cv_bridge import CvBridge from nav_msgs.msg import MapMetaData, OccupancyGrid from geometry_msgs.msg import Pose from scr.node import Node from scr.states import DeviceStateEnum -from sensor_msgs.msg import CompressedImage from nav_msgs.msg import OccupancyGrid @@ -29,116 +26,49 @@ IMAGE_HEIGHT = 480 -class ImageCombinerConfig: - def __init__(self): - self.map_res = 80 - self.top_left = [147, 287] - self.top_right = [851, 232] - self.bottom_right = [955, 678] - self.bottom_left = [0, 678] - class ImageCombiner(Node): def __init__(self): - super().__init__("autonav_image_combiner") + super().__init__("autonav_vision_combiner") def init(self): - self.image_left = None - self.image_right = None - self.image_left_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.image_received_left, self.qos_profile) - self.image_right_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.image_received_right, self.qos_profile) - self.combined_image_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/raw", self.qos_profile) - self.combined_image_publisher_debug = self.create_publisher(CompressedImage, "/autonav/cfg_space/raw/debug", self.qos_profile) + self.grid_left = None + self.grid_right = None + self.grid_left_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/left", self.grid_received_left, self.qos_profile) + self.grid_right_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/right", self.grid_received_right, self.qos_profile) + self.combined_grid_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/combined", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) - def image_received_left(self, msg): - self.image_left = msg - self.try_combine_images() - - def image_received_right(self, msg): - self.image_right = msg - self.try_combine_images() - - def order_points(self, pts): - # initialzie a list of coordinates that will be ordered - # such that the first entry in the list is the top-left, - # the second entry is the top-right, the third is the - # bottom-right, and the fourth is the bottom-left - rect = np.zeros((4, 2), dtype = "float32") - # the top-left point will have the smallest sum, whereas - # the bottom-right point will have the largest sum - s = pts.sum(axis = 1) - rect[0] = pts[np.argmin(s)] - rect[2] = pts[np.argmax(s)] - # now, compute the difference between the points, the - # top-right point will have the smallest difference, - # whereas the bottom-left will have the largest difference - diff = np.diff(pts, axis = 1) - rect[1] = pts[np.argmin(diff)] - rect[3] = pts[np.argmax(diff)] - # return the ordered coordinates - return rect - - - def four_point_transform(self, image, pts): - # obtain a consistent order of the points and unpack them - # individually - rect = self.order_points(pts) - (tl, tr, br, bl) = rect - # compute the width of the new image, which will be the - # maximum distance between bottom-right and bottom-left - # x-coordiates or the top-right and top-left x-coordinates - widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2)) - widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2)) - maxWidth = max(int(widthA), int(widthB)) - # compute the height of the new image, which will be the - # maximum distance between the top-right and bottom-right - # y-coordinates or the top-left and bottom-left y-coordinates - heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2)) - heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2)) - maxHeight = max(int(heightA), int(heightB)) - # now that we have the dimensions of the new image, construct - # the set of destination points to obtain a "birds eye view", - # (i.e. top-down view) of the image, again specifying points - # in the top-left, top-right, bottom-right, and bottom-left - # order - dst = np.array([ - [0, 0], - [maxWidth - 1, 0], - [maxWidth - 1, maxHeight - 1], - [0, maxHeight - 1]], dtype="float32") - # compute the perspective transform matrix and then apply it - M = cv2.getPerspectiveTransform(rect, dst) - warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) - # return the warped image - return warped - - def try_combine_images(self): - if self.image_left is None or self.image_right is None: + def grid_received_left(self, msg): + self.grid_left = msg + self.try_combine_grids() + + def grid_received_right(self, msg): + self.grid_right = msg + self.try_combine_grids() + + def try_combine_grids(self): + if self.grid_left is None or self.grid_right is None: return - # Combine the images to form (IMAGE_WIDTH * 2) x IMAGE_HEIGHT - # The left image will be on the left, and the right image will be on the right - cv2img_left = g_bridge.compressed_imgmsg_to_cv2(self.image_left) - cv2img_right = g_bridge.compressed_imgmsg_to_cv2(self.image_right) - combined = cv2.hconcat([cv2img_left, cv2img_right]) - # ordered as: top-left, top-right, bottom-right, bottom-left - pts = [self.config.top_left, self.config.top_right, self.config.bottom_right, self.config.bottom_left] - combined = self.four_point_transform(combined, np.array(pts)) - datamap = cv2.resize(combined, dsize=(self.config.map_res, self.config.map_res), interpolation=cv2.INTER_LINEAR) / 2 - flat = list(datamap.flatten().astype(int)) - msg = OccupancyGrid(info=g_mapData, data=flat) - - self.image_left = None - self.image_right = None - self.combined_image_publisher.publish(msg) - compressed_msg = g_bridge.cv2_to_compressed_imgmsg(combined) - self.combined_image_publisher_debug.publish(compressed_msg) + combined_grid = OccupancyGrid() + combined_grid.header = self.grid_left.header + combined_grid.info = self.grid_left.info + combined_grid.data = [0] * len(self.grid_left.data) + for i in range(len(self.grid_left.data)): + if self.grid_left.data[i] > 0 or self.grid_right.data[i] > 0: + # Need to figure out what the value actually is: 255, 100, 1? + combined_grid.data[i] = self.grid_left.data[i] if self.grid_left.data[i] > self.grid_right.data[i] else self.grid_right.data[i] + else: + combined_grid.data[i] = 0 + + self.grid_left = None + self.grid_right = None def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) def get_default_config(self): - return ImageCombinerConfig() + return {} def main(): diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 096c78c5..4f6cfb5e 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -43,7 +43,7 @@ class ExpandifyNode : public SCR::Node build_circles(); - raw_map_subscriber = create_subscription("/autonav/cfg_space/raw", qos_profile, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); + raw_map_subscriber = create_subscription("/autonav/cfg_space/combined", qos_profile, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", qos_profile); debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", qos_profile); diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 0c5186bb..d51bd511 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -36,9 +36,24 @@ def __init__(self): self.upper_val = 210 self.blur_weight = 5 self.blur_iterations = 3 - self.rod_offset = 130 self.map_res = 80 + # Default to entire image + self.left_topleft = [0, 0] + self.left_topright = [640, 0] + self.left_bottomright = [640, 480] + self.left_bottomleft = [0, 480] + + self.right_topleft = [0, 0] + self.right_topright = [640, 0] + self.right_bottomright = [640, 480] + self.right_bottomleft = [0, 480] + + self.disable_blur = False + self.disable_hsv = False + self.disable_region_of_disinterest = False + self.disable_perspective_transform = False + class ImageTransformer(Node): def __init__(self, dir = "left"): @@ -50,7 +65,7 @@ def directionify(self, topic): def init(self): self.cameraSubscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, self.qos_profile) - self.rawMapPublisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/preraw"), self.qos_profile) + self.rawMapPublisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/raw"), self.qos_profile) self.filteredImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image"), self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) @@ -66,6 +81,60 @@ def getBlur(self): blur = max(1, blur) return (blur, blur) + def order_points(self, pts): + # initialzie a list of coordinates that will be ordered + # such that the first entry in the list is the top-left, + # the second entry is the top-right, the third is the + # bottom-right, and the fourth is the bottom-left + rect = np.zeros((4, 2), dtype = "float32") + # the top-left point will have the smallest sum, whereas + # the bottom-right point will have the largest sum + s = pts.sum(axis = 1) + rect[0] = pts[np.argmin(s)] + rect[2] = pts[np.argmax(s)] + # now, compute the difference between the points, the + # top-right point will have the smallest difference, + # whereas the bottom-left will have the largest difference + diff = np.diff(pts, axis = 1) + rect[1] = pts[np.argmin(diff)] + rect[3] = pts[np.argmax(diff)] + # return the ordered coordinates + return rect + + + def four_point_transform(self, image, pts): + # obtain a consistent order of the points and unpack them + # individually + rect = self.order_points(pts) + (tl, tr, br, bl) = rect + # compute the width of the new image, which will be the + # maximum distance between bottom-right and bottom-left + # x-coordiates or the top-right and top-left x-coordinates + widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2)) + widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2)) + maxWidth = max(int(widthA), int(widthB)) + # compute the height of the new image, which will be the + # maximum distance between the top-right and bottom-right + # y-coordinates or the top-left and bottom-left y-coordinates + heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2)) + heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2)) + maxHeight = max(int(heightA), int(heightB)) + # now that we have the dimensions of the new image, construct + # the set of destination points to obtain a "birds eye view", + # (i.e. top-down view) of the image, again specifying points + # in the top-left, top-right, bottom-right, and bottom-left + # order + dst = np.array([ + [0, 0], + [maxWidth - 1, 0], + [maxWidth - 1, maxHeight - 1], + [0, maxHeight - 1]], dtype="float32") + # compute the perspective transform matrix and then apply it + M = cv2.getPerspectiveTransform(rect, dst) + warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) + # return the warped image + return warped + def regionOfDisinterest(self, img, vertices): mask = np.ones_like(img) * 255 cv2.fillPoly(mask, vertices, 0) @@ -83,42 +152,49 @@ def onImageReceived(self, image = CompressedImage): cv_image = g_bridge.compressed_imgmsg_to_cv2(image) # Blur it up - for _ in range(self.config.blur_iterations): - cv_image = cv2.blur(cv_image, self.getBlur()) + if not self.config.disable_blur: + for _ in range(self.config.blur_iterations): + cv_image = cv2.blur(cv_image, self.getBlur()) # Apply filter and return a mask img = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) - lower = (self.config.lower_hue, self.config.lower_sat, self.config.lower_val) - upper = (self.config.upper_hue, self.config.upper_sat, self.config.upper_val) - mask = cv2.inRange(img, lower, upper) - mask = 255 - mask - - # Apply region of disinterest and flattening - height = img.shape[0] - width = img.shape[1] - if self.dir == "left": - region_of_disinterest_vertices = [ - (width, height), - (width, height / 1.8), - (0, height) - ] + if not self.config.disable_hsv: + lower = (self.config.lower_hue, self.config.lower_sat, self.config.lower_val) + upper = (self.config.upper_hue, self.config.upper_sat, self.config.upper_val) + mask = cv2.inRange(img, lower, upper) + mask = 255 - mask else: - region_of_disinterest_vertices = [ - (0, height), - (0, height / 1.8), - (width, height) - ] + mask = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) # Apply region of disinterest and flattening - mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) - mask[mask < 250] = 0 + if not self.config.disable_region_of_disinterest: + height = img.shape[0] + width = img.shape[1] + if self.dir == "left": + region_of_disinterest_vertices = [ + (width, height), + (width, height / 1.8), + (0, height) + ] + else: + region_of_disinterest_vertices = [ + (0, height), + (0, height / 1.8), + (width, height) + ] + mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) + mask[mask < 250] = 0 + + # Apply perspective transform + if not self.config.disable_perspective_transform: + pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] + mask = self.four_point_transform(mask, np.array(pts)) # Actually generate the map self.publishOccupancyMap(mask) # Preview the image preview_image = cv2.cvtColor(mask, cv2.COLOR_GRAY2RGB) - # cv2.polylines(preview_image, np.array([region_of_disinterest_vertices], np.int32), True, (0, 255, 0), 2) preview_msg = g_bridge.cv2_to_compressed_imgmsg(preview_image) preview_msg.header = image.header preview_msg.format = "jpeg" From daad96851c7893f0ac8f3f31fff4df2ce6f51062 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 1 May 2024 18:17:54 -0500 Subject: [PATCH 029/113] More work on image stuff --- autonav_ws/src/autonav_display/src/display.py | 51 ++++++++++++++++++- .../src/autonav_vision/src/combination.py | 24 +++++++-- .../src/autonav_vision/src/transformations.py | 29 +++++------ display/index.html | 6 ++- display/scripts/globals.js | 21 +++++--- display/scripts/main.js | 20 +++++++- display/styles/index.css | 7 ++- 7 files changed, 126 insertions(+), 32 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 81630b1f..69244f98 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -64,6 +64,8 @@ def __init__(self): self.limiter.setLimit("/autonav/camera/compressed/right", 2) self.limiter.setLimit("/autonav/cfg_space/raw/image/left", 5) self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/left_small", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/right_small", 5) self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) self.limiter.setLimit("/autonav/debug/astar/image", 5) @@ -92,8 +94,11 @@ def __init__(self): self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.cameraCallbackLeft, self.qos_profile) self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.cameraCallbackRight, self.qos_profile) - self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, self.qos_profile) - self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, self.qos_profile) + self.filteredSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, self.qos_profile) + self.filteredSubscriberRight = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, self.qos_profile) + self.filteredSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left_small", self.filteredCallbackLeftSmall, self.qos_profile) + self.filteredSubscriberRight = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right_small", self.filteredCallbackRightSmall, self.qos_profile) + self.filteredSubscriberCombined = self.create_subscription(CompressedImage, "/autonav/cfg_space/combined/image", self.filteredCallbackCombined, self.qos_profile) self.bigboiSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.bigboiCallback, self.qos_profile) self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, self.qos_profile) @@ -406,6 +411,48 @@ def filteredCallbackRight(self, msg: CompressedImage): "data": base64_str })) + def filteredCallbackLeftSmall(self, msg: CompressedImage): + if not self.limiter.use("/autonav/cfg_space/raw/image/left_small"): + return + + byts = msg.data.tobytes() + base64_str = base64.b64encode(byts).decode("utf-8") + + self.pushSendQueue(json.dumps({ + "op": "data", + "topic": "/autonav/cfg_space/raw/image/left_small", + "format": msg.format, + "data": base64_str + })) + + def filteredCallbackRightSmall(self, msg: CompressedImage): + if not self.limiter.use("/autonav/cfg_space/raw/image/right_small"): + return + + byts = msg.data.tobytes() + base64_str = base64.b64encode(byts).decode("utf-8") + + self.pushSendQueue(json.dumps({ + "op": "data", + "topic": "/autonav/cfg_space/raw/image/right_small", + "format": msg.format, + "data": base64_str + })) + + def filteredCallbackCombined(self, msg: CompressedImage): + if not self.limiter.use("/autonav/cfg_space/combined/image"): + return + + byts = msg.data.tobytes() + base64_str = base64.b64encode(byts).decode("utf-8") + + self.pushSendQueue(json.dumps({ + "op": "data", + "topic": "/autonav/cfg_space/combined/image", + "format": msg.format, + "data": base64_str + })) + def bigboiCallback(self, msg: CompressedImage): if not self.limiter.use("/autonav/cfg_space/raw/debug"): return diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 49fef4ab..fcf082a0 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -3,8 +3,11 @@ from types import SimpleNamespace import rclpy import json +import cv2 +import numpy as np from cv_bridge import CvBridge from nav_msgs.msg import MapMetaData, OccupancyGrid +from sensor_msgs.msg import CompressedImage from geometry_msgs.msg import Pose from scr.node import Node @@ -36,6 +39,7 @@ def init(self): self.grid_left_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/left", self.grid_received_left, self.qos_profile) self.grid_right_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/right", self.grid_received_right, self.qos_profile) self.combined_grid_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/combined", self.qos_profile) + self.combined_grid_image_publisher = self.create_publisher(CompressedImage, "/autonav/cfg_space/combined/image", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) def grid_received_left(self, msg): @@ -55,14 +59,28 @@ def try_combine_grids(self): combined_grid.info = self.grid_left.info combined_grid.data = [0] * len(self.grid_left.data) for i in range(len(self.grid_left.data)): - if self.grid_left.data[i] > 0 or self.grid_right.data[i] > 0: - # Need to figure out what the value actually is: 255, 100, 1? - combined_grid.data[i] = self.grid_left.data[i] if self.grid_left.data[i] > self.grid_right.data[i] else self.grid_right.data[i] + if self.grid_left.data[i] == 127 or self.grid_right.data[i] == 127: + combined_grid.data[i] = 127 else: combined_grid.data[i] = 0 + # Publish the combined grid self.grid_left = None self.grid_right = None + self.combined_grid_publisher.publish(combined_grid) + + # publish debug image, 127 = white, 0 = black + preview_image = np.zeros((IMAGE_HEIGHT, IMAGE_WIDTH, 3), np.uint8) + for i in range(len(combined_grid.data)): + x = i % combined_grid.info.width + y = i // combined_grid.info.width + if combined_grid.data[i] == 127: + preview_image[y, x] = [255, 255, 255] + else: + preview_image[y, x] = [0, 0, 0] + + compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) + self.combined_grid_image_publisher.publish(compressed_image) def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index d51bd511..c80c017b 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -40,14 +40,14 @@ def __init__(self): # Default to entire image self.left_topleft = [0, 0] - self.left_topright = [640, 0] - self.left_bottomright = [640, 480] - self.left_bottomleft = [0, 480] + self.left_topright = [480, 0] + self.left_bottomright = [480, 640] + self.left_bottomleft = [0, 640] self.right_topleft = [0, 0] - self.right_topright = [640, 0] - self.right_bottomright = [640, 480] - self.right_bottomleft = [0, 480] + self.right_topright = [480, 0] + self.right_bottomright = [480, 640] + self.right_bottomleft = [0, 640] self.disable_blur = False self.disable_hsv = False @@ -66,7 +66,7 @@ def directionify(self, topic): def init(self): self.cameraSubscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, self.qos_profile) self.rawMapPublisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/raw"), self.qos_profile) - self.filteredImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image"), self.qos_profile) + self.smallImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image") + "_small", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) @@ -147,6 +147,12 @@ def publishOccupancyMap(self, img): msg = OccupancyGrid(info=g_mapData, data=flat) self.rawMapPublisher.publish(msg) + # Publish msg as a 80x80 image + preview_image = cv2.resize(img, dsize=(80, 80), interpolation=cv2.INTER_LINEAR) + preview_msg = g_bridge.cv2_to_compressed_imgmsg(preview_image) + preview_msg.format = "jpeg" + self.smallImagePublisher.publish(preview_msg) + def onImageReceived(self, image = CompressedImage): # Decompressify cv_image = g_bridge.compressed_imgmsg_to_cv2(image) @@ -193,15 +199,6 @@ def onImageReceived(self, image = CompressedImage): # Actually generate the map self.publishOccupancyMap(mask) - # Preview the image - preview_image = cv2.cvtColor(mask, cv2.COLOR_GRAY2RGB) - preview_msg = g_bridge.cv2_to_compressed_imgmsg(preview_image) - preview_msg.header = image.header - preview_msg.format = "jpeg" - - # Publish the preview - self.filteredImagePublisher.publish(preview_msg) - def main(): rclpy.init() diff --git a/display/index.html b/display/index.html index 0e27006c..1fc8e800 100644 --- a/display/index.html +++ b/display/index.html @@ -125,8 +125,10 @@

    Camera / Filtered

    - - + + + + diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 5fde2813..f56c7865 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -43,8 +43,19 @@ var addressKeys = { "upper_val": "int", "blur_weight": "int", "blur_iterations": "int", - "rod_offset": "int", - "map_res": "int" + "map_res": "int", + "left_bottomleft": "point.int", + "left_bottomright": "point.int", + "left_topleft": "point.int", + "left_topright": "point.int", + "right_bottomleft": "point.int", + "right_bottomright": "point.int", + "right_topleft": "point.int", + "right_topright": "point.int", + "disable_blur": "bool", + "disable_hsv": "bool", + "disable_perspective_transform": "bool", + "disable_region_of_disinterest": "bool" }, "autonav_vision_expandifier": { @@ -110,11 +121,7 @@ var addressKeys = { "autonav_image_combiner": { "internal_title": "[Image Combiner]", - "map_res": "int", - "top_left": "point.int", - "top_right": "point.int", - "bottom_right": "point.int", - "bottom_left": "point.int" + "map_res": "int" } } diff --git a/display/scripts/main.js b/display/scripts/main.js index b8fad84a..1451026e 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -418,6 +418,24 @@ $(document).ready(function () { return; } + if (topic == "/autonav/cfg_space/raw/image/left_small") { + const imgElement = document.getElementById("target_filtered_left"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + + if (topic == "/autonav/cfg_space/raw/image/right_small") { + const imgElement = document.getElementById("target_filtered_right"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + + if (topic == "/autonav/cfg_space/combined/image") { + const imgElement = document.getElementById("target_combined"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + if (topic == "/autonav/cfg_space/raw/debug") { const imgElement = document.getElementById("target_camera_raw"); imgElement.src = `data:image/jpeg;base64,${msg.data}`; @@ -425,7 +443,7 @@ $(document).ready(function () { } if (topic == "/autonav/debug/astar/image") { - const imgElement = document.getElementById("target_astar_path"); + const imgElement = document.getElementById("target_expandified"); imgElement.src = `data:image/jpeg;base64,${msg.data}`; return; } diff --git a/display/styles/index.css b/display/styles/index.css index 2db3d8cd..89a40a7f 100644 --- a/display/styles/index.css +++ b/display/styles/index.css @@ -163,11 +163,16 @@ span[data-state="5"] { height: 480px !important; } -#images > canas[data-type="bigboy"] { +#images > canvas[data-type="bigboy"] { width: 800px !important; height: 800px !important; } +#images > canvas[data-type="small"] { + width: 80px !important; + height: 80px !important; +} + #logging { margin: 1rem; } \ No newline at end of file From 01dda0149a20f2ecaadc5978b648ff56a568d4c6 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 1 May 2024 18:40:39 -0500 Subject: [PATCH 030/113] Add testing stuff --- autonav_ws/src/autonav_display/src/display.py | 1 + .../src/autonav_vision/src/transformations.py | 27 +++++++++++++------ 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 69244f98..e176dad9 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -66,6 +66,7 @@ def __init__(self): self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5) self.limiter.setLimit("/autonav/cfg_space/raw/image/left_small", 5) self.limiter.setLimit("/autonav/cfg_space/raw/image/right_small", 5) + self.limiter.setLimit("/autonav/cfg_space/combined/image", 5) self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) self.limiter.setLimit("/autonav/debug/astar/image", 5) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index c80c017b..e77db507 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -38,16 +38,27 @@ def __init__(self): self.blur_iterations = 3 self.map_res = 80 + # # Default to entire image + # self.left_topleft = [0, 0] + # self.left_topright = [480, 0] + # self.left_bottomright = [480, 640] + # self.left_bottomleft = [0, 640] + + # self.right_topleft = [0, 0] + # self.right_topright = [480, 0] + # self.right_bottomright = [480, 640] + # self.right_bottomleft = [0, 640] + # Default to entire image - self.left_topleft = [0, 0] - self.left_topright = [480, 0] - self.left_bottomright = [480, 640] - self.left_bottomleft = [0, 640] + self.left_topleft = [0, 150] + self.left_topright = [640, 150] + self.left_bottomright = [640, 350] + self.left_bottomleft = [0, 350] - self.right_topleft = [0, 0] - self.right_topright = [480, 0] - self.right_bottomright = [480, 640] - self.right_bottomleft = [0, 640] + self.right_topleft = [0, 150] + self.right_topright = [640, 150] + self.right_bottomright = [640, 350] + self.right_bottomleft = [0, 350] self.disable_blur = False self.disable_hsv = False From ee47df916cf7332c191b342fa6346d95c8e631b1 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 1 May 2024 18:41:34 -0500 Subject: [PATCH 031/113] update rgb lights mode --- autonav_ws/src/autonav_serial/src/serial_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index 7cf8a3d2..c4f5af46 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -161,7 +161,7 @@ def onSafetyLightsReceived(self, lights: SafetyLights): packed_data = SafetyLightsPacket() packed_data.autonomous = lights.autonomous packed_data.eco = False - packed_data.mode = lights.mode + packed_data.mode = 1 packed_data.brightness = lights.brightness packed_data.red = lights.red packed_data.green = lights.green From 230aa1eb9868626cfbd216459fcc55e0c14e2393 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 1 May 2024 18:47:58 -0500 Subject: [PATCH 032/113] Change lights mode to solid always --- autonav_ws/src/autonav_serial/src/serial_node.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index c4f5af46..eeec2120 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -161,7 +161,7 @@ def onSafetyLightsReceived(self, lights: SafetyLights): packed_data = SafetyLightsPacket() packed_data.autonomous = lights.autonomous packed_data.eco = False - packed_data.mode = 1 + packed_data.mode = 0 packed_data.brightness = lights.brightness packed_data.red = lights.red packed_data.green = lights.green From ee59e6c5b84bdb6203832f7b7010fa547fb43f81 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 1 May 2024 18:57:43 -0500 Subject: [PATCH 033/113] Update image transformation coordinates in transformations.py --- .../src/autonav_vision/src/transformations.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index e77db507..424beb01 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -50,15 +50,15 @@ def __init__(self): # self.right_bottomleft = [0, 640] # Default to entire image - self.left_topleft = [0, 150] - self.left_topright = [640, 150] - self.left_bottomright = [640, 350] - self.left_bottomleft = [0, 350] + self.left_topleft = [0, 225] + self.left_topright = [640, 225] + self.left_bottomright = [640, 480] + self.left_bottomleft = [0, 480] - self.right_topleft = [0, 150] - self.right_topright = [640, 150] - self.right_bottomright = [640, 350] - self.right_bottomleft = [0, 350] + self.right_topleft = [0, 225] + self.right_topright = [640, 225] + self.right_bottomright = [640, 480] + self.right_bottomleft = [0, 480] self.disable_blur = False self.disable_hsv = False From c3c7ce59f94c4ea7e6a3d138ff2c0661932f5007 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 13 May 2024 10:30:17 -0500 Subject: [PATCH 034/113] Lower default auto speed --- autonav_ws/src/autonav_nav/src/path_resolver.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/path_resolver.py b/autonav_ws/src/autonav_nav/src/path_resolver.py index 16778c8a..c497ae29 100644 --- a/autonav_ws/src/autonav_nav/src/path_resolver.py +++ b/autonav_ws/src/autonav_nav/src/path_resolver.py @@ -39,12 +39,12 @@ def toSafetyLights(autonomous: bool, eco: bool, mode: int, brightness: int, colo class PathResolverNodeConfig: def __init__(self): - self.forward_speed = 2.1 + self.forward_speed = 1.0 self.reverse_speed = -0.4 self.radius_multiplier = 1.2 self.radius_max = 4.0 self.radius_start = 0.7 - self.angular_aggression = 2.2 + self.angular_aggression = 1.2 self.max_angular_speed = 0.5 From 4c5cfbf7750860781453f4a46f19525a1e577a7f Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 13 May 2024 10:45:34 -0500 Subject: [PATCH 035/113] Fix perspective transform to actually be perspective --- .../src/autonav_vision/src/expandify.cpp | 4 +-- .../src/autonav_vision/src/transformations.py | 35 +++++++++---------- display/scripts/globals.js | 4 ++- 3 files changed, 21 insertions(+), 22 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 4f6cfb5e..46aefa83 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -176,8 +176,8 @@ class ExpandifyNode : public SCR::Node nav_msgs::msg::MapMetaData map; - float maxRange = 0.65; - float noGoPercent = 0.70; + float maxRange = 0.55; + float noGoPercent = 0.60; int noGoRange = 0; std::vector circles; ExpandifyConfig config; diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 424beb01..7e1b0375 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -28,38 +28,34 @@ class ImageTransformerConfig: def __init__(self): + # HSV self.lower_hue = 0 self.lower_sat = 0 self.lower_val = 0 self.upper_hue = 255 self.upper_sat = 95 self.upper_val = 210 + + # Blur self.blur_weight = 5 self.blur_iterations = 3 self.map_res = 80 - # # Default to entire image - # self.left_topleft = [0, 0] - # self.left_topright = [480, 0] - # self.left_bottomright = [480, 640] - # self.left_bottomleft = [0, 640] - - # self.right_topleft = [0, 0] - # self.right_topright = [480, 0] - # self.right_bottomright = [480, 640] - # self.right_bottomleft = [0, 640] - - # Default to entire image - self.left_topleft = [0, 225] - self.left_topright = [640, 225] + # Perspective transform + self.left_topleft = [160, 225] + self.left_topright = [400, 225] self.left_bottomright = [640, 480] self.left_bottomleft = [0, 480] - - self.right_topleft = [0, 225] - self.right_topright = [640, 225] + self.right_topleft = [160, 225] + self.right_topright = [400, 225] self.right_bottomright = [640, 480] self.right_bottomleft = [0, 480] + # Region of disinterest + self.rodi_offsetx = 0 + self.rodi_offsety = 0 + + # Disabling self.disable_blur = False self.disable_hsv = False self.disable_region_of_disinterest = False @@ -69,6 +65,7 @@ def __init__(self): class ImageTransformer(Node): def __init__(self, dir = "left"): super().__init__("autonav_vision_transformer") + self.config = self.get_default_config() self.dir = dir def directionify(self, topic): @@ -195,8 +192,8 @@ def onImageReceived(self, image = CompressedImage): ] else: region_of_disinterest_vertices = [ - (0, height), - (0, height / 1.8), + (self.config.rodi_offsetx, height), + (0, height / 1.8 - self.config.rodi_offsety), (width, height) ] mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) diff --git a/display/scripts/globals.js b/display/scripts/globals.js index f56c7865..6c5e070c 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -55,7 +55,9 @@ var addressKeys = { "disable_blur": "bool", "disable_hsv": "bool", "disable_perspective_transform": "bool", - "disable_region_of_disinterest": "bool" + "disable_region_of_disinterest": "bool", + "rodi_offsetx": "float", + "rodi_offsety": "float" }, "autonav_vision_expandifier": { From 4ecb418bd41fcda1c88b386626decb2da748c4ce Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 13 May 2024 11:55:11 -0500 Subject: [PATCH 036/113] Update region of disinterest and combination method --- .../src/autonav_vision/src/combination.py | 50 +++++++++++++------ .../src/autonav_vision/src/transformations.py | 8 +-- 2 files changed, 39 insertions(+), 19 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index fcf082a0..fac728a0 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -50,35 +50,55 @@ def grid_received_right(self, msg): self.grid_right = msg self.try_combine_grids() + def scale_grid(self, grid): + # Initialize a new grid of the same height but half the width + scaled_grid = OccupancyGrid() + scaled_grid.info = grid.info + scaled_grid.info.width = 40 + scaled_grid.data = [0] * (80 * 80) + + # Copy every second column from the original grid to the new grid + for y in range(80): + for x in range(40): + # Take every second element from the row + scaled_grid.data[y * 40 + x] = grid.data[y * 80 + (2 * x)] + + return scaled_grid + def try_combine_grids(self): if self.grid_left is None or self.grid_right is None: return + + # Scale down grids + scaled_left = self.scale_grid(self.grid_left) + scaled_right = self.scale_grid(self.grid_right) + # Create a new 80x80 grid for the combined result combined_grid = OccupancyGrid() - combined_grid.header = self.grid_left.header - combined_grid.info = self.grid_left.info - combined_grid.data = [0] * len(self.grid_left.data) - for i in range(len(self.grid_left.data)): - if self.grid_left.data[i] == 127 or self.grid_right.data[i] == 127: - combined_grid.data[i] = 127 - else: - combined_grid.data[i] = 0 + combined_grid.info = g_mapData + combined_grid.info.width = 80 + combined_grid.info.height = 80 + combined_grid.data = [0] * (80 * 80) + + # Place each grid in the combined grid + for y in range(80): + for x in range(40): # Fill from left scaled grid + combined_grid.data[y * 80 + x] = scaled_left.data[y * 40 + x] + for x in range(40): # Fill from right scaled grid + combined_grid.data[y * 80 + (40 + x)] = scaled_right.data[y * 40 + x] # Publish the combined grid self.grid_left = None self.grid_right = None self.combined_grid_publisher.publish(combined_grid) - # publish debug image, 127 = white, 0 = black - preview_image = np.zeros((IMAGE_HEIGHT, IMAGE_WIDTH, 3), np.uint8) + # Publish the combined grid as an image + preview_image = np.zeros((IMAGE_HEIGHT, IMAGE_WIDTH), np.uint8) for i in range(len(combined_grid.data)): x = i % combined_grid.info.width y = i // combined_grid.info.width - if combined_grid.data[i] == 127: - preview_image[y, x] = [255, 255, 255] - else: - preview_image[y, x] = [0, 0, 0] - + if combined_grid.data[i] > 0: + preview_image[y, x] = 255 compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) self.combined_grid_image_publisher.publish(compressed_image) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 7e1b0375..c3d554c9 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -188,13 +188,13 @@ def onImageReceived(self, image = CompressedImage): region_of_disinterest_vertices = [ (width, height), (width, height / 1.8), - (0, height) + (width / 3, height) ] else: region_of_disinterest_vertices = [ - (self.config.rodi_offsetx, height), - (0, height / 1.8 - self.config.rodi_offsety), - (width, height) + (0, height), + (0, height / 1.8), + (width - (width / 3), height) ] mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) mask[mask < 250] = 0 From 24080a2d68c906a8cba378bd8f9580d6b80bf973 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 13 May 2024 13:04:52 -0500 Subject: [PATCH 037/113] Remove extra tabs --- autonav_ws/src/autonav_vision/src/combination.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index fac728a0..a28c72cf 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -56,13 +56,13 @@ def scale_grid(self, grid): scaled_grid.info = grid.info scaled_grid.info.width = 40 scaled_grid.data = [0] * (80 * 80) - + # Copy every second column from the original grid to the new grid for y in range(80): for x in range(40): # Take every second element from the row scaled_grid.data[y * 40 + x] = grid.data[y * 80 + (2 * x)] - + return scaled_grid def try_combine_grids(self): @@ -72,14 +72,14 @@ def try_combine_grids(self): # Scale down grids scaled_left = self.scale_grid(self.grid_left) scaled_right = self.scale_grid(self.grid_right) - + # Create a new 80x80 grid for the combined result combined_grid = OccupancyGrid() combined_grid.info = g_mapData combined_grid.info.width = 80 combined_grid.info.height = 80 combined_grid.data = [0] * (80 * 80) - + # Place each grid in the combined grid for y in range(80): for x in range(40): # Fill from left scaled grid From 18a67c00c0d469ff2eaff0d7b18ede97c7efa186 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 13 May 2024 15:02:05 -0500 Subject: [PATCH 038/113] scrabby works --- .../src/autonav_launch/launch/simulation.xml | 1 + .../src/autonav_playback/CMakeLists.txt | 21 ++ .../autonav_playback/__init__.py | 0 autonav_ws/src/autonav_playback/package.xml | 17 ++ .../src/autonav_playback/src/playback.py | 274 ++++++++++++++++++ .../src/autonav_vision/src/transformations.py | 8 +- 6 files changed, 317 insertions(+), 4 deletions(-) create mode 100644 autonav_ws/src/autonav_playback/CMakeLists.txt create mode 100644 autonav_ws/src/autonav_playback/autonav_playback/__init__.py create mode 100644 autonav_ws/src/autonav_playback/package.xml create mode 100644 autonav_ws/src/autonav_playback/src/playback.py diff --git a/autonav_ws/src/autonav_launch/launch/simulation.xml b/autonav_ws/src/autonav_launch/launch/simulation.xml index dfe15047..276cc986 100644 --- a/autonav_ws/src/autonav_launch/launch/simulation.xml +++ b/autonav_ws/src/autonav_launch/launch/simulation.xml @@ -26,4 +26,5 @@ + \ No newline at end of file diff --git a/autonav_ws/src/autonav_playback/CMakeLists.txt b/autonav_ws/src/autonav_playback/CMakeLists.txt new file mode 100644 index 00000000..1c654a44 --- /dev/null +++ b/autonav_ws/src/autonav_playback/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(autonav_playback) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + src/playback.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/autonav_ws/src/autonav_playback/autonav_playback/__init__.py b/autonav_ws/src/autonav_playback/autonav_playback/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/autonav_ws/src/autonav_playback/package.xml b/autonav_ws/src/autonav_playback/package.xml new file mode 100644 index 00000000..d69062ba --- /dev/null +++ b/autonav_ws/src/autonav_playback/package.xml @@ -0,0 +1,17 @@ + + + + autonav_playback + 1.0.0 + Contains nodes relevant to playing back previously recorded runs + Dylan Zemlin + MIT + + ament_cmake_python + ros2launch + + rclpy + + ament_cmake + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_playback/src/playback.py b/autonav_ws/src/autonav_playback/src/playback.py new file mode 100644 index 00000000..558d872d --- /dev/null +++ b/autonav_ws/src/autonav_playback/src/playback.py @@ -0,0 +1,274 @@ +#!/usr/bin/env python3 + +from types import SimpleNamespace +from autonav_msgs.msg import IMUData, GPSFeedback, MotorFeedback, MotorInput, Position, MotorControllerDebug, ObjectDetection +from scr.states import DeviceStateEnum, SystemStateEnum +from sensor_msgs.msg import CompressedImage +from scr_msgs.msg import SystemState, DeviceState +from cv_bridge import CvBridge +from scr.node import Node +from datetime import datetime +import shutil +import rclpy +import cv2 +import os +import json + + +class ImageTransformerConfig: + def __init__(self): + self.record_imu = True + self.record_gps = True + self.record_position = True + self.record_feedback = True + self.record_motor_debug = True + self.record_raw_cameras = True + self.record_filtered_cameras = True + self.record_astar = True + self.record_autonomous = True + self.record_manual = True + self.frame_rate = 8 + + +class PlaybackNode(Node): + def __init__(self): + super().__init__("autonav_playback") + + self.startTime = datetime.now().timestamp() + self.file = None + self.file_name = None + self.home_dir = os.path.expanduser("~") + self.camera_index = 0 + self.filtered_index = 0 + self.astar_index = 0 + self.bridge = CvBridge() + self.config = self.get_default_config() + self.image_raw_left = None + self.image_raw_right = None + self.image_filtered_left = None + self.image_filtered_right = None + + def init(self): + self.imu_subscriber = self.create_subscription(IMUData, "/autonav/imu", self.imu_callback, 20) + self.gps_subscriber = self.create_subscription(GPSFeedback, "/autonav/gps", self.gps_callback, 20) + self.feedback_subscriber = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.feedback_callback, 20) + self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.position_callback, 20) + self.controller_debug_subscriber = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.controller_debug_callback, 20) + self.thresholded_subscriber_left = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left_small", self.filtered_callback_left, self.qos_profile) + self.thresholded_subscriber_right = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right_small", self.filtered_callback_right, self.qos_profile) + self.camera_subscriber_left = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.camera_callback_left, self.qos_profile) + self.camera_subscriber_right = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.camera_callback_right, self.qos_profile) + self.astar_subscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.astar_callback, self.qos_profile) + + self.set_device_state(DeviceStateEnum.OPERATING) + + def config_updated(self, jsonObject): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + + def get_default_config(self): + return ImageTransformerConfig() + + def timestamp(self): + return datetime.now().timestamp() + + def create_file(self): + time = datetime.now() + timeFrmt = time.strftime("%Y-%m-%d_%H-%M-%S") + stateFrmt = "autonomous" if self.system_state == SystemStateEnum.AUTONOMOUS else "manual" + return f"{stateFrmt}_{timeFrmt}" + + def create_video(self, folder, name): + IMAGES_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name, "images", folder) + SAVE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name) + os.system(f"ffmpeg -r {self.config.frame_rate} -i {IMAGES_PATH}/%d.jpg -vcodec libx264 -crf 18 -pix_fmt yuv420p -y {SAVE_PATH}/{name}.mp4") + + def create_entry(self): + self.file_name = self.create_file() + self.startTime = datetime.now().timestamp() + + BASE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name) + os.makedirs(BASE_PATH, exist_ok=True) + os.makedirs(os.path.join(BASE_PATH, "images", "thresholded"), exist_ok=True) + os.makedirs(os.path.join(BASE_PATH, "images", "astar"), exist_ok=True) + os.makedirs(os.path.join(BASE_PATH, "images", "camera"), exist_ok=True) + + self.file = open(os.path.join(BASE_PATH, "log.csv"), "w") + self.file.write("timestamp, type\n") + + self.filtered_index = 0 + self.astar_index = 0 + self.camera_index = 0 + + self.log(f"Recording playback data at {BASE_PATH}") + + def close_entry(self): + if self.file is None: + return + + self.file.close() + self.file = None + + # Zip up the folder at $HOME/.scr/playback/{fileName} and then delete it + BASE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name) + self.create_video("thresholded", "thresholded") + self.create_video("astar", "astar") + self.create_video("camera", "camera") + + # For every type of log in the log file, create a seperate csv file for it + with open(os.path.join(BASE_PATH, "log.csv"), "r") as logFile: + for line in logFile.readlines()[1:]: + logEntry = line.split(", ") + logType = logEntry[1].strip() + logPath = os.path.join(BASE_PATH, f"{logType}.csv") + with open(logPath, "a") as logTypeFile: + logTypeFile.write(line) + + # Delete the images folder + shutil.rmtree(os.path.join(BASE_PATH, "images"), ignore_errors=True) + + shutil.make_archive(BASE_PATH, "zip", BASE_PATH) + SIZE_OF_ZIP = os.path.getsize(BASE_PATH + ".zip") / 1024 / 1024 + TIME_ELAPSED = datetime.now().timestamp() - self.startTime + self.log(f"Saved playback data to {self.file_name}.zip ({SIZE_OF_ZIP:.3f} MB) over {TIME_ELAPSED:.3f} seconds") + shutil.rmtree(BASE_PATH, ignore_errors=True) + + def write_file(self, msg: str): + if self.file is None: + return + + self.file.write(msg + "\n") + + def write_image(self, img: CompressedImage, relative_path: str, idx: int): + if self.file_name is None: + return + + IMAGE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name, "images", relative_path) + cv2Image = self.bridge.compressed_imgmsg_to_cv2(img, "bgr8") + cv2.imwrite(os.path.join(IMAGE_PATH, f"{idx}.jpg"), cv2Image) + + def system_state_transition(self, old: SystemState, updated: SystemState): + if old.state == SystemStateEnum.AUTONOMOUS and updated.state != SystemStateEnum.AUTONOMOUS: + self.close_entry() + + if old.state == SystemStateEnum.MANUAL and updated.state != SystemStateEnum.MANUAL: + self.close_entry() + + if old.state != SystemStateEnum.AUTONOMOUS and updated.state == SystemStateEnum.AUTONOMOUS and self.config.record_autonomous: + self.create_entry() + + if old.state != SystemStateEnum.MANUAL and updated.state == SystemStateEnum.MANUAL and self.config.record_manual: + self.create_entry() + + def imu_callback(self, msg: IMUData): + if not self.config.record_imu: + return + + self.write_file(f"{self.timestamp()}, ENTRY_IMU, {msg.accel_x}, {msg.accel_y}, {msg.accel_z}, {msg.angular_x}, {msg.angular_y}, {msg.angular_z}, {msg.roll}, {msg.pitch}, {msg.yaw}") + + def gps_callback(self, msg: GPSFeedback): + if not self.config.record_gps: + return + + self.write_file(f"{self.timestamp()}, ENTRY_GPS, {msg.latitude}, {msg.longitude}, {msg.altitude}, {msg.gps_fix}, {msg.is_locked}, {msg.satellites}") + + def feedback_callback(self, msg: MotorFeedback): + if not self.config.record_feedback: + return + + self.write_file(f"{self.timestamp()}, ENTRY_FEEDBACK, {msg.delta_x}, {msg.delta_y}, {msg.delta_theta}") + + def position_callback(self, msg: Position): + if not self.config.record_position: + return + + self.write_file(f"{self.timestamp()}, ENTRY_POSITION, {msg.x}, {msg.y}, {msg.theta}, {msg.latitude}, {msg.longitude}") + + def controller_debug_callback(self, msg: MotorControllerDebug): + if not self.config.record_motor_debug: + return + + self.write_file(f"{self.timestamp()}, ENTRY_MOTORDEBUG, {msg.current_forward_velocity}, {msg.forward_velocity_setpoint}, {msg.current_angular_velocity}, {msg.angular_velocity_setpoint}, {msg.left_motor_output}, {msg.right_motor_output}") + + def filtered_callback_left(self, msg: CompressedImage): + if not self.config.record_filtered_cameras: + return + + self.image_filtered_left = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + if self.image_filtered_right is not None: + self.filtered_callback_combined() + + def filtered_callback_right(self, msg: CompressedImage): + if not self.config.record_filtered_cameras: + return + + self.image_filtered_right = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + if self.image_filtered_left is not None: + self.filtered_callback_combined() + + def filtered_callback_combined(self): + if not self.config.record_filtered_cameras: + return + + # Scale the images to the 800 x 800 + self.image_filtered_left = cv2.resize(self.image_filtered_left, (800, 800)) + self.image_filtered_right = cv2.resize(self.image_filtered_right, (800, 800)) + + img = cv2.hconcat([self.image_filtered_left, self.image_filtered_right]) + msg = self.bridge.cv2_to_compressed_imgmsg(img) + self.write_file(f"{self.timestamp()}, ENTRY_FILTERED_IMAGE, /images/thresholded/{self.filtered_index}.jpg") + self.write_image(msg, "thresholded", self.filtered_index) + self.filtered_index += 1 + self.image_filtered_left = None + self.image_filtered_right = None + + def astar_callback(self, msg: CompressedImage): + if not self.config.record_astar: + return + + # Scale the images to the 800 x 800 + img = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + img = cv2.resize(img, (800, 800)) + msg = self.bridge.cv2_to_compressed_imgmsg(img) + + self.write_file(f"{self.timestamp()}, ENTRY_ASTAR_IMAGE, /images/expandified/{self.astar_index}.jpg") + self.write_image(msg, "astar", self.astar_index) + self.astar_index += 1 + + def camera_callback_left(self, msg: CompressedImage): + if not self.config.record_raw_cameras: + return + + self.image_raw_left = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + if self.image_raw_right is not None: + self.camera_callback_combined() + + def camera_callback_right(self, msg: CompressedImage): + if not self.config.record_raw_cameras: + return + + self.image_raw_right = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + if self.image_raw_left is not None: + self.camera_callback_combined() + + def camera_callback_combined(self): + if not self.config.record_raw_cameras: + return + + img = cv2.hconcat([self.image_raw_left, self.image_raw_right]) + msg = self.bridge.cv2_to_compressed_imgmsg(img) # Bad but works + self.write_file(f"{self.timestamp()}, ENTRY_CAMERA_IMAGE, /images/camera/{self.camera_index}.jpg") + self.write_image(msg, "camera", self.camera_index) + self.camera_index += 1 + self.image_raw_left = None + self.image_raw_right = None + + +def main(): + rclpy.init() + node = PlaybackNode() + Node.run_node(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index c3d554c9..78bedad1 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -42,12 +42,12 @@ def __init__(self): self.map_res = 80 # Perspective transform - self.left_topleft = [160, 225] - self.left_topright = [400, 225] + self.left_topleft = [160, 165] + self.left_topright = [400, 165] self.left_bottomright = [640, 480] self.left_bottomleft = [0, 480] - self.right_topleft = [160, 225] - self.right_topright = [400, 225] + self.right_topleft = [160, 165] + self.right_topright = [400, 165] self.right_bottomright = [640, 480] self.right_bottomleft = [0, 480] From 77400177847f51c0b31997d3d53b87fb7ac20dd7 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:23:36 -0500 Subject: [PATCH 039/113] Fix debug astar not showing --- autonav_ws/src/autonav_display/src/display.py | 20 ++++--------------- 1 file changed, 4 insertions(+), 16 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index e176dad9..f7ac51c0 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -69,6 +69,7 @@ def __init__(self): self.limiter.setLimit("/autonav/cfg_space/combined/image", 5) self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) self.limiter.setLimit("/autonav/debug/astar/image", 5) + self.limiter.setLimit("/autonav/debug/astar", 5) self.get_logger().info("Broadcasting on ws://{}:{}".format(self.host, self.port)) @@ -84,8 +85,7 @@ def __init__(self): self.motorFeedbackSubscriber = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.motorFeedbackCallback, 20) self.motorInputSubscriber = self.create_subscription(MotorInput, "/autonav/MotorInput", self.motorInputCallback, 20) self.motorControllerDebugSubscriber = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.motorControllerDebugCallback, 20) - self.objectDetectionSubscriber = self.create_subscription(ObjectDetection, "/autonav/ObjectDetection", self.objectDetectionCallback, 20) - self.pathingDebugSubscriber = self.create_subscription(PathingDebug, "/autonav/cfg_space/expanded/image", self.pathingDebugCallback, 20) + self.pathingDebugSubscriber = self.create_subscription(PathingDebug, "/autonav/debug/astar", self.pathingDebugCallback, 20) self.gpsFeedbackSubscriber = self.create_subscription(GPSFeedback, "/autonav/gps", self.gpsFeedbackCallback, 20) self.imuDataSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.imuDataCallback, 20) self.conbusSubscriber = self.create_subscription(Conbus, "/autonav/conbus/data", self.conbusCallback, 100) @@ -93,8 +93,8 @@ def __init__(self): self.systemStateService = self.create_client(SetSystemState, "/scr/state/set_system_state") - self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.cameraCallbackLeft, self.qos_profile) - self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.cameraCallbackRight, self.qos_profile) + self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left/cutout", self.cameraCallbackLeft, self.qos_profile) + self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right/cutout", self.cameraCallbackRight, self.qos_profile) self.filteredSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, self.qos_profile) self.filteredSubscriberRight = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, self.qos_profile) self.filteredSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left_small", self.filteredCallbackLeftSmall, self.qos_profile) @@ -329,18 +329,6 @@ def pathingDebugCallback(self, msg: PathingDebug): "time_until_use_waypoints": msg.time_until_use_waypoints, })) - def objectDetectionCallback(self, msg: ObjectDetection): - if not self.limiter.use("/autonav/ObjectDetection"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/ObjectDetection", - "sensor_1": msg.sensor_1, - "sensor_2": msg.sensor_2, - "sensor_3": msg.sensor_3 - })) - def motorControllerDebugCallback(self, msg: MotorControllerDebug): if not self.limiter.use("/autonav/MotorControllerDebug"): return From 2680f03c0d9bef9753a78aae4360ee4377b74b57 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:24:38 -0500 Subject: [PATCH 040/113] Clean up astar file a tad --- autonav_ws/src/autonav_nav/src/astar.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 6730a049..76790f28 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -71,7 +71,7 @@ def __init__(self): self.waypointPopDistance = 1.1 self.waypointDirection = 0 self.useOnlyWaypoints = False - self.waypointDelay = 10 + self.waypointDelay = 18 class AStarNode(Node): @@ -90,12 +90,12 @@ def get_default_config(self): return AStarNodeConfig() def init(self): - self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.onConfigSpaceReceived, self.qos_profile) - self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.onPoseReceived, self.qos_profile) - self.imuSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.onImuReceived, self.qos_profile) - self.debugPublisher = self.create_publisher(PathingDebug, "/autonav/debug/astar", self.qos_profile) - self.pathPublisher = self.create_publisher(Path, "/autonav/path", self.qos_profile) - self.safetyLightsPublisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", self.qos_profile) + self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.onConfigSpaceReceived, 20) + self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.onPoseReceived, 20) + self.imuSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.onImuReceived, 20) + self.debugPublisher = self.create_publisher(PathingDebug, "/autonav/debug/astar", 20) + self.pathPublisher = self.create_publisher(Path, "/autonav/path", 20) + self.safetyLightsPublisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 20) self.pathDebugImagePublisher = self.create_publisher(CompressedImage, "/autonav/debug/astar/image", self.qos_profile) self.mapTimer = self.create_timer(0.1, self.createPath) @@ -135,7 +135,7 @@ def getWaypointsForDirection(self): return simulation_waypoints[direction_index] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[direction_index] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[direction_index] - def transition(self, old: SystemState, updated: SystemState): + def system_state_transition(self, old: SystemState, updated: SystemState): if updated.state == SystemStateEnum.AUTONOMOUS and updated.mobility and len(self.waypoints) == 0: self.waypointTime = time.time() + self.config.waypointDelay From e4401e42694ffdb8b39b84faad38ec881c762477 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:24:53 -0500 Subject: [PATCH 041/113] Update default path resolver config --- autonav_ws/src/autonav_nav/src/path_resolver.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/path_resolver.py b/autonav_ws/src/autonav_nav/src/path_resolver.py index c497ae29..7ff5f8fe 100644 --- a/autonav_ws/src/autonav_nav/src/path_resolver.py +++ b/autonav_ws/src/autonav_nav/src/path_resolver.py @@ -39,13 +39,13 @@ def toSafetyLights(autonomous: bool, eco: bool, mode: int, brightness: int, colo class PathResolverNodeConfig: def __init__(self): - self.forward_speed = 1.0 + self.forward_speed = 1.5 self.reverse_speed = -0.4 self.radius_multiplier = 1.2 self.radius_max = 4.0 self.radius_start = 0.7 - self.angular_aggression = 1.2 - self.max_angular_speed = 0.5 + self.angular_aggression = 1.8 + self.max_angular_speed = 0.8 class PathResolverNode(Node): @@ -57,10 +57,10 @@ def init(self): self.pure_pursuit = PurePursuit() self.backCount = -1 self.status = -1 - self.path_subscriber = self.create_subscription(Path, "/autonav/path", self.on_path_received, self.qos_profile) - self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.on_position_received, self.qos_profile) + self.path_subscriber = self.create_subscription(Path, "/autonav/path", self.on_path_received, 1) + self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.on_position_received, 1) self.motor_publisher = self.create_publisher(MotorInput, "/autonav/MotorInput", 1) - self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", self.qos_profile) + self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 1) self.config = self.get_default_config() self.create_timer(0.05, self.onResolve) From 70f268fcedcc0464cf588694a00a79ffdd776741 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:25:02 -0500 Subject: [PATCH 042/113] Add new config options to camera serial --- autonav_ws/src/autonav_serial/src/camera.py | 81 +++++++++++++++++---- 1 file changed, 66 insertions(+), 15 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 3987f423..9f65b17f 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -24,13 +24,27 @@ def __init__(self): self.camera_index_right = 2 self.scan_rate = 1.0 + self.left_flip_horizontal = True + self.left_flip_vertical = True + self.right_flip_horizontal = True + self.right_flip_vertical = True + self.rotate_left_clockwise = True + self.rotate_right_clockwise = True class CameraNode(Node): def __init__(self): super().__init__("autonav_serial_camera") self.camera_publisher_left = self.create_publisher(CompressedImage, "/autonav/camera/compressed/left", 20) self.camera_publisher_right = self.create_publisher(CompressedImage, "/autonav/camera/compressed/right", 20) + self.lock_left = threading.Lock() + self.lock_right = threading.Lock() + self.left_kill = False + self.right_kill = False + + self.autofocus = 0 + self.autoexposure = 0 + def init(self): self.get_logger().info("Initializing camera node...") self.create_threads() @@ -44,18 +58,61 @@ def create_threads(self): self.camera_thread_right.daemon = True self.camera_thread_right.start() + def destroy_threads(self): + self.lock_left.acquire() + self.left_kill = True + self.lock_left.release() + + self.lock_right.acquire() + self.right_kill = True + self.lock_right.release() + + self.camera_thread_left.join() + self.camera_thread_right.join() + + self.lock_left.acquire() + self.left_kill = False + self.lock_left.release() + + self.lock_right.acquire() + self.right_kill = False + self.lock_right.release() + def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) def get_default_config(self): return CameraNodeConfig() + + def apply_transformations(self, frame, dir): + if dir == "left": + if self.config.left_flip_horizontal: + frame = cv2.flip(frame, 1) + if self.config.left_flip_vertical: + frame = cv2.flip(frame, 0) + if self.config.rotate_left_clockwise: + frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) + else: + frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) + else: + if self.config.right_flip_horizontal: + frame = cv2.flip(frame, 1) + if self.config.right_flip_vertical: + frame = cv2.flip(frame, 0) + if self.config.rotate_right_clockwise: + frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) + else: + frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) + + frame.resize(self.config.output_width, self.config.output_height) + return frame def camera_worker(self, *args, **kwargs): index_name = args[0] if len(args) > 0 else "" camera_index = self.config.camera_index_left if index_name == "left" else self.config.camera_index_right capture = None - while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: + while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN and (index_name == "left" and not self.left_kill or index_name == "right" and not self.right_kill): try: if not os.path.exists("/dev/video" + str(camera_index)): time.sleep(self.config.scan_rate) @@ -68,29 +125,22 @@ def camera_worker(self, *args, **kwargs): capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.config.output_width) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.config.output_height) - # self.set_device_state(DeviceStateEnum.OPERATING) + capture.set(cv2.CAP_PROP_AUTOFOCUS, self.autofocus) + capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, self.autoexposure) except: - # self.set_device_state(DeviceStateEnum.STANDBY) time.sleep(self.config.scan_rate) continue while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: - # if self.device_state != DeviceStateEnum.OPERATING: - # continue - try: ret, frame = capture.read() - if index_name == "left": - frame = cv2.flip(frame, 1) - frame = cv2.flip(frame, 0) - frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) - frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) + self.apply_transformations(frame, index_name) except: - if capture is not None: - capture.release() - capture = None + if capture is None: + break - # self.set_device_state(DeviceStateEnum.STANDBY) + capture.release() + capture = None break if not ret or frame is None: @@ -100,6 +150,7 @@ def camera_worker(self, *args, **kwargs): self.camera_publisher_left.publish(bridge.cv2_to_compressed_imgmsg(frame)) else: self.camera_publisher_right.publish(bridge.cv2_to_compressed_imgmsg(frame)) + time.sleep(1.0 / self.config.refresh_rate) From 3682c1fdac52b4641b421344683358d940eee279 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:25:18 -0500 Subject: [PATCH 043/113] Adjust subscription qos --- autonav_ws/src/autonav_vision/src/combination.py | 6 +++--- autonav_ws/src/autonav_vision/src/expandify.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index a28c72cf..33af510d 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -36,9 +36,9 @@ def __init__(self): def init(self): self.grid_left = None self.grid_right = None - self.grid_left_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/left", self.grid_received_left, self.qos_profile) - self.grid_right_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/right", self.grid_received_right, self.qos_profile) - self.combined_grid_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/combined", self.qos_profile) + self.grid_left_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/left", self.grid_received_left, 1) + self.grid_right_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/right", self.grid_received_right, 1) + self.combined_grid_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/combined", 1) self.combined_grid_image_publisher = self.create_publisher(CompressedImage, "/autonav/cfg_space/combined/image", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 46aefa83..696dffd6 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -43,8 +43,8 @@ class ExpandifyNode : public SCR::Node build_circles(); - raw_map_subscriber = create_subscription("/autonav/cfg_space/combined", qos_profile, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); - expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", qos_profile); + raw_map_subscriber = create_subscription("/autonav/cfg_space/combined", 1, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); + expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", 1); debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", qos_profile); set_device_state(SCR::DeviceState::OPERATING); @@ -92,9 +92,9 @@ class ExpandifyNode : public SCR::Node { ExpandifyConfig newConfig; newConfig.vertical_fov = 2.75; - newConfig.horizontal_fov = 3; + newConfig.horizontal_fov = 3.4; newConfig.map_res = 80.0f; - newConfig.max_range = 0.65; + newConfig.max_range = 0.73; newConfig.no_go_percent = 0.70; newConfig.no_go_range = 0; return newConfig; From 8dfbca3908643e64d3506ba02d4e1bd940874fdc Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:25:31 -0500 Subject: [PATCH 044/113] Add new options for vision adjustement and visual lines --- .../src/autonav_vision/src/transformations.py | 137 +++++++++++------- display/scripts/globals.js | 4 +- display/scripts/main.js | 125 ++++++++++++++++ 3 files changed, 211 insertions(+), 55 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 78bedad1..9a113cd8 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -10,6 +10,7 @@ from sensor_msgs.msg import CompressedImage from geometry_msgs.msg import Pose from cv_bridge import CvBridge +from scr.states import SystemModeEnum import json from scr.node import Node @@ -42,18 +43,19 @@ def __init__(self): self.map_res = 80 # Perspective transform - self.left_topleft = [160, 165] - self.left_topright = [400, 165] - self.left_bottomright = [640, 480] - self.left_bottomleft = [0, 480] - self.right_topleft = [160, 165] - self.right_topright = [400, 165] - self.right_bottomright = [640, 480] - self.right_bottomleft = [0, 480] + self.left_topleft = [80, 220] + self.left_topright = [400, 220] + self.left_bottomright = [480, 640] + self.left_bottomleft = [0, 640] + self.right_topleft = [80, 220] + self.right_topright = [400, 220] + self.right_bottomright = [480, 640] + self.right_bottomleft = [0, 640] # Region of disinterest - self.rodi_offsetx = 0 - self.rodi_offsety = 0 + # Order: top-left, top-right, bottom-right, bottom-left + self.parallelogram_left = [[500, 405], [260, 400], [210, 640], [640, 640]] + self.parallelogram_right = [[0, 415], [195, 390], [260, 640], [0, 640]] # Disabling self.disable_blur = False @@ -72,9 +74,10 @@ def directionify(self, topic): return topic + "/" + self.dir def init(self): - self.cameraSubscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, self.qos_profile) - self.rawMapPublisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/raw"), self.qos_profile) - self.smallImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image") + "_small", self.qos_profile) + self.camera_subscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, self.qos_profile) + self.camera_debug_publisher = self.create_publisher(CompressedImage, self.directionify("/autonav/camera/compressed") + "/cutout", self.qos_profile) + self.grid_publisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/raw"), 1) + self.grid_image_publisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image") + "_small", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) @@ -84,7 +87,7 @@ def config_updated(self, jsonObject): def get_default_config(self): return ImageTransformerConfig() - def getBlur(self): + def get_blur_level(self): blur = self.config.blur_weight blur = max(1, blur) return (blur, blur) @@ -149,63 +152,91 @@ def regionOfDisinterest(self, img, vertices): masked_image = cv2.bitwise_and(img, mask) return masked_image - def publishOccupancyMap(self, img): + def publish_occupancy_grid(self, img): datamap = cv2.resize(img, dsize=(self.config.map_res, self.config.map_res), interpolation=cv2.INTER_LINEAR) / 2 flat = list(datamap.flatten().astype(int)) msg = OccupancyGrid(info=g_mapData, data=flat) - self.rawMapPublisher.publish(msg) + self.grid_publisher.publish(msg) # Publish msg as a 80x80 image preview_image = cv2.resize(img, dsize=(80, 80), interpolation=cv2.INTER_LINEAR) preview_msg = g_bridge.cv2_to_compressed_imgmsg(preview_image) preview_msg.format = "jpeg" - self.smallImagePublisher.publish(preview_msg) + self.grid_image_publisher.publish(preview_msg) - def onImageReceived(self, image = CompressedImage): - # Decompressify - cv_image = g_bridge.compressed_imgmsg_to_cv2(image) + def publish_debug_image(self, img): + img_copy = img.copy() + + # Draw parallelogram for region of disinterest + parallelogram = self.config.parallelogram_left if self.dir == "left" else self.config.parallelogram_right + cv2.polylines(img_copy, [np.array(parallelogram)], True, (0, 255, 0), 2) + + # Draw perspective transform points + pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] + cv2.polylines(img_copy, [np.array(pts)], True, (0, 0, 255), 2) + + self.camera_debug_publisher.publish(g_bridge.cv2_to_compressed_imgmsg(img_copy)) + + def apply_blur(self, img): + if self.config.disable_blur: + return img + + for _ in range(self.config.blur_iterations): + img = cv2.blur(img, self.get_blur_level()) + + return img + + def apply_hsv(self, img): + if self.config.disable_hsv: + return img + + img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) + lower = (self.config.lower_hue, self.config.lower_sat, self.config.lower_val) + upper = (self.config.upper_hue, self.config.upper_sat, self.config.upper_val) + mask = cv2.inRange(img, lower, upper) + mask = 255 - mask + return mask + + def apply_region_of_disinterest(self, img): + if self.config.disable_region_of_disinterest: + return img + + mask = np.ones_like(img) * 255 + parallelogram = self.config.parallelogram_left if self.dir == "left" else self.config.parallelogram_right + cv2.fillPoly(mask, [np.array(parallelogram)], 0) + mask = cv2.bitwise_and(img, mask) + mask[mask < 250] = 0 + return mask + + def apply_perspective_transform(self, img): + if self.config.disable_perspective_transform: + return img + + pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] + mask = self.four_point_transform(img, np.array(pts)) + return mask + + def onImageReceived(self, image: CompressedImage): + # Decompress image + img = g_bridge.compressed_imgmsg_to_cv2(image) + + # Publish debug image + self.publish_debug_image(img) # Blur it up - if not self.config.disable_blur: - for _ in range(self.config.blur_iterations): - cv_image = cv2.blur(cv_image, self.getBlur()) + img = self.apply_blur(img) # Apply filter and return a mask - img = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) - if not self.config.disable_hsv: - lower = (self.config.lower_hue, self.config.lower_sat, self.config.lower_val) - upper = (self.config.upper_hue, self.config.upper_sat, self.config.upper_val) - mask = cv2.inRange(img, lower, upper) - mask = 255 - mask - else: - mask = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY) + img = self.apply_hsv(img) # Apply region of disinterest and flattening - if not self.config.disable_region_of_disinterest: - height = img.shape[0] - width = img.shape[1] - if self.dir == "left": - region_of_disinterest_vertices = [ - (width, height), - (width, height / 1.8), - (width / 3, height) - ] - else: - region_of_disinterest_vertices = [ - (0, height), - (0, height / 1.8), - (width - (width / 3), height) - ] - mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) - mask[mask < 250] = 0 - + img = self.apply_region_of_disinterest(img) + # Apply perspective transform - if not self.config.disable_perspective_transform: - pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] - mask = self.four_point_transform(mask, np.array(pts)) + img = self.apply_perspective_transform(img) # Actually generate the map - self.publishOccupancyMap(mask) + self.publish_occupancy_grid(img) def main(): diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 6c5e070c..fd0e2694 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -56,8 +56,8 @@ var addressKeys = { "disable_hsv": "bool", "disable_perspective_transform": "bool", "disable_region_of_disinterest": "bool", - "rodi_offsetx": "float", - "rodi_offsety": "float" + "parallelogram_left": "parallelogram.int", + "parallelogram_right": "parallelogram.int", }, "autonav_vision_expandifier": { diff --git a/display/scripts/main.js b/display/scripts/main.js index 1451026e..673fbe77 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -700,6 +700,131 @@ $(document).ready(function () { div.appendChild(inputY); return div; } + else if (type == "parallelogram.int") { + // 4 boxes with 2 integers each + const div = document.createElement("div"); + div.classList.add("input-group"); + div.classList.add("mb-3"); + + const inputX1 = document.createElement("input"); + inputX1.type = "number"; + inputX1.classList.add("form-control"); + inputX1.value = data[0][0]; + inputX1.onchange = function () { + config[device][text][0][0] = parseInt(inputX1.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputY1 = document.createElement("input"); + inputY1.type = "number"; + inputY1.classList.add("form-control"); + inputY1.value = data[0][1]; + inputY1.onchange = function () { + config[device][text][0][1] = parseInt(inputY1.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputX2 = document.createElement("input"); + inputX2.type = "number"; + inputX2.classList.add("form-control"); + inputX2.value = data[1][0]; + inputX2.onchange = function () { + config[device][text][1][0] = parseInt(inputX2.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputY2 = document.createElement("input"); + inputY2.type = "number"; + inputY2.classList.add("form-control"); + inputY2.value = data[1][1]; + inputY2.onchange = function () { + config[device][text][1][1] = parseInt(inputY2.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputX3 = document.createElement("input"); + inputX3.type = "number"; + inputX3.classList.add("form-control"); + inputX3.value = data[2][0]; + inputX3.onchange = function () { + config[device][text][2][0] = parseInt(inputX3.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputY3 = document.createElement("input"); + inputY3.type = "number"; + inputY3.classList.add("form-control"); + inputY3.value = data[2][1]; + inputY3.onchange = function () { + config[device][text][2][1] = parseInt(inputY3.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputX4 = document.createElement("input"); + inputX4.type = "number"; + inputX4.classList.add("form-control"); + inputX4.value = data[3][0]; + inputX4.onchange = function () { + config[device][text][3][0] = parseInt(inputX4.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputY4 = document.createElement("input"); + inputY4.type = "number"; + inputY4.classList.add("form-control"); + inputY4.value = data[3][1]; + inputY4.onchange = function () { + config[device][text][3][1] = parseInt(inputY4.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const span = document.createElement("span"); + span.classList.add("input-group-text"); + span.innerText = text; + + div.appendChild(span); + div.appendChild(inputX1); + div.appendChild(inputY1); + div.appendChild(inputX2); + div.appendChild(inputY2); + div.appendChild(inputX3); + div.appendChild(inputY3); + div.appendChild(inputX4); + div.appendChild(inputY4); + return div; + } else { const options = addressKeys[device][text]; From 59916cfdf6c0d8e7ca4588dcf676b1ed047cce02 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:34:25 -0500 Subject: [PATCH 045/113] Clean up particle filter a bit --- autonav_ws/src/autonav_filters/src/filters.py | 87 +++----- .../src/autonav_filters/src/particlefilter.py | 40 ++-- autonav_ws/src/autonav_vision/CMakeLists.txt | 1 - .../src/autonav_vision/src/circumscriber.py | 202 ------------------ 4 files changed, 54 insertions(+), 276 deletions(-) delete mode 100644 autonav_ws/src/autonav_vision/src/circumscriber.py diff --git a/autonav_ws/src/autonav_filters/src/filters.py b/autonav_ws/src/autonav_filters/src/filters.py index 27af58bb..ca1c0fd9 100644 --- a/autonav_ws/src/autonav_filters/src/filters.py +++ b/autonav_ws/src/autonav_filters/src/filters.py @@ -12,6 +12,7 @@ import rclpy import math + class FilterType(IntEnum): DEAD_RECKONING = 0, PARTICLE_FILTER = 1 @@ -20,25 +21,22 @@ class FilterType(IntEnum): class FiltersNodeConfig: def __init__(self): self.filter_type = FilterType.PARTICLE_FILTER - self.degree_offset = 107.0 - self.seed_heading = False class FiltersNode(Node): def __init__(self): super().__init__("autonav_filters") - self.lastIMUReceived = None - self.firstGps = None - self.lastGps = None - - self.latitudeLength = self.declare_parameter("latitude_length", 111086.2).get_parameter_value().double_value - self.longitudeLength = self.declare_parameter("longitude_length", 81978.2).get_parameter_value().double_value - - self.pf = ParticleFilter(self.latitudeLength, self.longitudeLength) - self.reckoning = DeadReckoningFilter() + self.first_gps = None + self.last_gps = None + + self.latitude_length = self.declare_parameter("latitude_length", 111086.2).get_parameter_value().double_value + self.longitude_length = self.declare_parameter("longitude_length", 81978.2).get_parameter_value().double_value + + self.pf = ParticleFilter(self.latitude_length, self.longitude_length) + self.dr = DeadReckoningFilter() self.config = self.get_default_config() - + self.onReset() def config_updated(self, jsonObject): @@ -49,29 +47,14 @@ def get_default_config(self): def init(self): self.create_subscription(GPSFeedback, "/autonav/gps", self.onGPSReceived, 20) - self.create_subscription(IMUData, "/autonav/imu", self.onIMUReceived, 20); self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.onMotorFeedbackReceived, 20) - self.positionPublisher = self.create_publisher(Position, "/autonav/position", 20) + self.position_publisher = self.create_publisher(Position, "/autonav/position", 20) self.set_device_state(DeviceStateEnum.OPERATING) - - def onIMUReceived(self, msg: IMUData): - self.lastIMUReceived = msg - - def getRealHeading(self, heading: float): - if heading < 0: - heading = 360 + -heading - - heading += self.config.degree_offset - return heading def onReset(self): - if self.lastIMUReceived is not None and self.config.seed_heading: - self.reckoning.reset(self.getRealHeading(self.lastIMUReceived.heading)) - self.pf.init_particles(self.getRealHeading(self.lastIMUReceived.heading), True) - else: - self.reckoning.reset() - self.pf.init_particles() + self.dr.reset() + self.pf.init_particles() def system_state_transition(self, old: SystemState, updated: SystemState): if old.state != SystemStateEnum.AUTONOMOUS and updated.state == SystemStateEnum.AUTONOMOUS: @@ -79,50 +62,46 @@ def system_state_transition(self, old: SystemState, updated: SystemState): if old.mobility == False and updated.mobility == True: self.onReset() - + def onGPSReceived(self, msg: GPSFeedback): if msg.gps_fix == 0 and msg.is_locked == False: return - - if self.firstGps is None: - self.firstGps = msg - self.lastGps = msg + if self.first_gps is None: + self.first_gps = msg - filterType = self.config.filter_type - if filterType == FilterType.PARTICLE_FILTER: + self.last_gps = msg + if self.config.filter_type == FilterType.PARTICLE_FILTER: self.pf.gps(msg) - elif filterType == FilterType.DEAD_RECKONING: - self.reckoning.gps(msg) + else: + self.dr.gps(msg) def onMotorFeedbackReceived(self, msg: MotorFeedback): - filterType = self.config.filter_type averages = None - if filterType == FilterType.PARTICLE_FILTER: + if self.config.filter_type == FilterType.PARTICLE_FILTER: averages = self.pf.feedback(msg) - if filterType == FilterType.DEAD_RECKONING: - averages = self.reckoning.feedback(msg) - + else: + averages = self.dr.feedback(msg) + if averages is None: return - + position = Position() position.x = averages[0] position.y = averages[1] position.theta = (-1 * math.pi * 2 + averages[2]) * 1 - - if self.firstGps is not None: - gps_x = self.firstGps.latitude + position.x / self.latitudeLength - gps_y = self.firstGps.longitude - position.y / self.longitudeLength + if self.first_gps is not None: + gps_x = self.first_gps.latitude + position.x / self.latitude_length + gps_y = self.first_gps.longitude - position.y / self.longitude_length position.latitude = gps_x position.longitude = gps_y - if self.system_mode == SystemModeEnum.SIMULATION and self.lastGps is not None: - position.latitude = self.lastGps.latitude - position.longitude = self.lastGps.longitude - - self.positionPublisher.publish(position) + if self.system_mode == SystemModeEnum.SIMULATION and self.last_gps is not None: + position.latitude = self.last_gps.latitude + position.longitude = self.last_gps.longitude + + self.position_publisher.publish(position) def main(): diff --git a/autonav_ws/src/autonav_filters/src/particlefilter.py b/autonav_ws/src/autonav_filters/src/particlefilter.py index 35f0b7c8..b88a6d8c 100644 --- a/autonav_ws/src/autonav_filters/src/particlefilter.py +++ b/autonav_ws/src/autonav_filters/src/particlefilter.py @@ -3,13 +3,15 @@ import math import random + class Particle: - def __init__(self, x = 0, y = 0, theta = 0, weight = 1) -> None: + def __init__(self, x=0, y=0, theta=0, weight=1) -> None: self.x = x self.y = y self.theta = theta self.weight = weight + class ParticleFilter: def __init__(self, latitudeLength, longitudeLength) -> None: self.num_particles = 750 @@ -17,9 +19,9 @@ def __init__(self, latitudeLength, longitudeLength) -> None: self.odom_noise = [0.05, 0.05, 0.1] self.init_particles() self.first_gps = None - - self.latitudeLength = latitudeLength - self.longitudeLength = longitudeLength + + self.latitude_length = latitudeLength + self.longitude_length = longitudeLength def init_particles(self, seedHeading: float = 0.0, useSeedHeading: bool = False): if useSeedHeading: @@ -33,7 +35,7 @@ def feedback(self, feedback: MotorFeedback) -> list[float]: sum_theta_x = 0 sum_theta_y = 0 sum_weight = 0 - + for particle in self.particles: particle.x += feedback.delta_x * 1.2 * math.cos(particle.theta) + feedback.delta_y * math.sin(particle.theta) particle.y += feedback.delta_x * 1.2 * math.sin(particle.theta) + feedback.delta_y * math.cos(particle.theta) @@ -45,44 +47,44 @@ def feedback(self, feedback: MotorFeedback) -> list[float]: sum_theta_x += math.cos(particle.theta) * weight sum_theta_y += math.sin(particle.theta) * weight sum_weight += weight - + if sum_weight < 0.000001: sum_weight = 0.000001 - + avg_x = sum_x / sum_weight avg_y = sum_y / sum_weight avg_theta = math.atan2(sum_theta_y / sum_weight, sum_theta_x / sum_weight) % (2 * math.pi) - + return [avg_x, avg_y, avg_theta] - + def gps(self, gps: GPSFeedback) -> list[float]: if self.first_gps is None: self.first_gps = gps - - gps_x = (gps.latitude - self.first_gps.latitude) * self.latitudeLength - gps_y = (self.first_gps.longitude - gps.longitude) * self.longitudeLength - + + gps_x = (gps.latitude - self.first_gps.latitude) * self.latitude_length + gps_y = (self.first_gps.longitude - gps.longitude) * self.longitude_length + for particle in self.particles: dist_sqrt = np.sqrt((particle.x - gps_x) ** 2 + (particle.y - gps_y) ** 2) particle.weight = math.exp(-dist_sqrt / (2 * self.gps_noise[0] ** 2)) - + self.resample() return [gps_x, gps_y] - + def resample(self) -> None: weights = [particle.weight for particle in self.particles] weights_sum = sum(weights) if weights_sum <= 0.00001: weights_sum = 0.00001 weights = [weight / weights_sum for weight in weights] - - new_particles = random.choices(self.particles, weights, k = self.num_particles) + + new_particles = random.choices(self.particles, weights, k=self.num_particles) self.particles = [] - + for particle in new_particles: rand_x = np.random.normal(0, self.odom_noise[0]) rand_y = np.random.normal(0, self.odom_noise[1]) x = particle.x + rand_x * math.cos(particle.theta) + rand_y * math.sin(particle.theta) y = particle.y + rand_x * math.sin(particle.theta) + rand_y * math.cos(particle.theta) theta = np.random.normal(particle.theta, self.odom_noise[2]) % (2 * math.pi) - self.particles.append(Particle(x, y, theta, particle.weight)) \ No newline at end of file + self.particles.append(Particle(x, y, theta, particle.weight)) diff --git a/autonav_ws/src/autonav_vision/CMakeLists.txt b/autonav_ws/src/autonav_vision/CMakeLists.txt index 205352b9..1bf2cd6d 100644 --- a/autonav_ws/src/autonav_vision/CMakeLists.txt +++ b/autonav_ws/src/autonav_vision/CMakeLists.txt @@ -21,7 +21,6 @@ find_package(image_transport REQUIRED) # Install Python programs install(PROGRAMS - src/circumscriber.py src/transformations.py src/combination.py DESTINATION lib/${PROJECT_NAME} diff --git a/autonav_ws/src/autonav_vision/src/circumscriber.py b/autonav_ws/src/autonav_vision/src/circumscriber.py deleted file mode 100644 index 4cb6ba10..00000000 --- a/autonav_ws/src/autonav_vision/src/circumscriber.py +++ /dev/null @@ -1,202 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import cv2 as cv -from enum import IntEnum -import rclpy -from rclpy.node import Node -from cv_bridge import CvBridge -import time -import math - -from sensor_msgs.msg import CompressedImage -from autonav_msgs.msg import Obstacle -from autonav_msgs.msg import Obstacles - -bridge = CvBridge() - -class Register(IntEnum): - LOWER_HUE = 0 - LOWER_SATURATION = 1 - LOWER_VALUE = 2 - UPPER_HUE = 3 - UPPER_SATURATION = 4 - UPPER_VALUE = 5 - BLUR = 6 - BLUR_ITERATIONS = 7 - APPLY_FLATTENING = 8 - APPLY_REGION_OF_INTEREST = 9 - -class Circumscriber(Node): - - def __init__(self): - super().__init__("circumscriber") - self.subscriber = self.create_subscription(CompressedImage, "/igvc/camera/compressed", self.on_image_received, 10) - self.publisher = self.create_publisher(CompressedImage, "/autonav/camera/filtered", 1) - self.obstacle_publisher = self.create_publisher(Obstacles, "/autonav/obstacles", 1) - self.h = None - self.w = None - - - def publish_obstacles(self, local_obstacle_list): - msg = Obstacles() - for local_obstacles in local_obstacle_list: - obstacle = Obstacle() - if local_obstacles[1] >= 240: - obstacle.center_x, obstacle.center_y, obstacle.radius = local_obstacles[0], local_obstacles[1] - 2 * (local_obstacles[1] - 240), local_obstacles[2] - else: - obstacle.center_x, obstacle.center_y, obstacle.radius = local_obstacles[0], local_obstacles[1] + 2 * (240 - local_obstacles[1]), local_obstacles[2] - - msg.obstacles_data.append(obstacle) - - self.obstacle_publisher.publish(msg) - self.get_logger().info(f"publishing {msg} as Obstacles to /autonav/obstacles") - - def on_image_received(self, image: CompressedImage): - - self.get_logger().info("Image received") - # Decompress - cv_image = bridge.compressed_imgmsg_to_cv2(image) - - # Blur it up - for _ in range(1): # Register.BLUR_ITERATIONS - cv_image = cv.blur(cv_image, self.get_blur()) - - # Apply filter and return a mask - img = cv.cvtColor(cv_image, cv.COLOR_BGR2HSV) - lower = ( - 0, # LOWER_HUE - 0, # LOWER_SATURATION - 35, #LOWER_VALUE - ) - upper = ( - 255, # UPPER_HUE - 100, # UPPER_SATURATION - 170, # UPPER_VALUE - ) - mask = cv.inRange(img, lower, upper) - mask = 255 - mask - mask[mask < 250] = 0 - - # Apply region of interest and flattening - height = img.shape[0] - width = img.shape[1] - region_of_interest_vertices = [ - (0, height), - (width / 2, height / 2 + 120), - (width, height), - ] - - map_image = mask - if True: # Register.APPLY_FLATTENING - map_image = self.region_of_interest(mask, np.array([region_of_interest_vertices], np.int32)) - - if True: # Register.APPLY_REGION_OF_INTEREST - map_image = self.flatten_image(mask) - - # Convert mask to RGB for preview - preview_image = map_image - cv.imshow("preview_image", preview_image) - cv.waitKey(1000) - - - start = time.time() - ret, thresh = cv.threshold(preview_image, 150, 255, 0) - # split the image into parts - h, w = thresh.shape - self.h, self.w = h, w - print(f"h = {h} w = {w}") - - # define how many sections of the image you want to search for objects in - # grid sizes need to be square numbers - - sections = 4 - grid_sections = sections ** 2 - fractional_w = int(w // sections) - fractional_h = int(h // sections) - - # for each grid section, find the contours and find the circles - # solve the grid problem - contour_collections = [] - for idx in range(grid_sections): - right_displacement = int(idx % sections) - left_displacement = int(sections - right_displacement - 1) - bottom_displacement = int(idx // sections) - top_displacement = int(sections - bottom_displacement - 1) - contours, _ = cv.findContours(thresh[fractional_h * bottom_displacement: h - (fractional_h * top_displacement), fractional_w * right_displacement : w - (fractional_w * left_displacement)], cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) - contour_collections.append(contours) - - - obstacles = [] - preview_image = cv.cvtColor(preview_image, cv.COLOR_GRAY2RGB) - # for the contours found, add them to the image - for collections in range(len(contour_collections)): - right_displacement = collections % sections - left_displacement = sections - right_displacement -1 - bottom_displacement = collections // sections - top_displacement = sections - bottom_displacement - 1 - - for cnt in contour_collections[collections]: - [x,y], radius = cv.minEnclosingCircle(cnt) - center = (int(x) + int(fractional_w * right_displacement), int(y) + int(fractional_h * bottom_displacement)) - #center = (int(x), int(y)) - radius = int(radius) - obstacles.append([center[0], center[1], radius]) - preview_image = cv.circle(preview_image, center, radius, (0,0,255), 2) - obstacles.append((center[0], center[1], radius)) - - # for testing whole or partial images - end = time.time() - self.get_logger().info(f"Time to draw circles: {end - start}") - - # display the image - cv.imshow("preview_image after circles", preview_image) - cv.waitKey(10000) - cv.destroyAllWindows() - - # send the obstacles to the path planner - self.publish_obstacles(obstacles) - preview_msg = bridge.cv2_to_compressed_imgmsg(preview_image) - preview_msg.header = image.header - preview_msg.format = "jpeg" - self.publisher.publish(preview_msg) - self.get_logger().info("Publishing an image") - - - def get_blur(self): - blur = 5 # Register.BLUR - blur = max(1, blur) - return (blur, blur) - - def region_of_interest(self, img, vertices): - mask = np.zeros_like(img) * 255 - match_mask_color = 0 - cv.fillPoly(mask, vertices, match_mask_color) - return cv.bitwise_and(img, mask) - - def flatten_image(self, img): - top_left = (int)(img.shape[1] * 0), (int)(img.shape[0] * 1) - top_right = (int)(img.shape[1] * 1), (int)(img.shape[0] * 1) - bottom_left = (int)(img.shape[1] * 0), (int)(img.shape[0] * 0) - bottom_right = (int)(img.shape[1] * 1), (int)(img.shape[0] * 0) - - src = np.float32([top_left, top_right, bottom_left, bottom_right]) - dst = np.float32([[0, img.shape[0]], [img.shape[1], img.shape[0]], [0, 0], [img.shape[1], 0]]) - - M = cv.getPerspectiveTransform(src, dst) - return cv.warpPerspective(img, M, (img.shape[1], img.shape[0])) - - -def main(args=None): - rclpy.init(args=args) - - circumscriber = Circumscriber() - - rclpy.spin(circumscriber) - - circumscriber.destroy_node() - - rclpy.shutdown() - -if __name__ == "__main__": - main() From 3152799e23b2582917207e10ac3f018ab5df3264 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:36:56 -0500 Subject: [PATCH 046/113] Some light launch file cleaning --- autonav_ws/src/autonav_launch/launch/competition.xml | 7 +------ .../src/autonav_launch/launch/managed_steam.xml | 11 +++-------- autonav_ws/src/autonav_launch/launch/practice.xml | 7 +------ autonav_ws/src/autonav_launch/launch/simulation.xml | 5 ++--- 4 files changed, 7 insertions(+), 23 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/competition.xml b/autonav_ws/src/autonav_launch/launch/competition.xml index 14e3e0d1..b10de6e2 100644 --- a/autonav_ws/src/autonav_launch/launch/competition.xml +++ b/autonav_ws/src/autonav_launch/launch/competition.xml @@ -1,17 +1,12 @@ - - - - - + - diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 16dd5890..c4fbc83e 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -1,17 +1,12 @@ - - - - - + - - - + + diff --git a/autonav_ws/src/autonav_launch/launch/practice.xml b/autonav_ws/src/autonav_launch/launch/practice.xml index 80476b3f..e22bea06 100644 --- a/autonav_ws/src/autonav_launch/launch/practice.xml +++ b/autonav_ws/src/autonav_launch/launch/practice.xml @@ -1,17 +1,12 @@ - - - - - + - diff --git a/autonav_ws/src/autonav_launch/launch/simulation.xml b/autonav_ws/src/autonav_launch/launch/simulation.xml index 276cc986..cdfcb914 100644 --- a/autonav_ws/src/autonav_launch/launch/simulation.xml +++ b/autonav_ws/src/autonav_launch/launch/simulation.xml @@ -10,9 +10,8 @@ - - - + + From 237cea75bcfbd0c17e25c6f86fe81138d82a1bcc Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 11:40:24 -0500 Subject: [PATCH 047/113] More cleanup --- autonav_ws/src/autonav_msgs/CMakeLists.txt | 1 - .../src/autonav_msgs/msg/ObjectDetection.msg | 3 - autonav_ws/src/autonav_nav/CMakeLists.txt | 1 - autonav_ws/src/autonav_nav/src/astar.py | 98 ++++++++----------- .../src/autonav_nav/src/path_resolver.py | 4 +- .../src/autonav_nav/src/pure_pursuit.py | 65 ------------ 6 files changed, 45 insertions(+), 127 deletions(-) delete mode 100644 autonav_ws/src/autonav_msgs/msg/ObjectDetection.msg delete mode 100644 autonav_ws/src/autonav_nav/src/pure_pursuit.py diff --git a/autonav_ws/src/autonav_msgs/CMakeLists.txt b/autonav_ws/src/autonav_msgs/CMakeLists.txt index ef949a6d..2babe12a 100644 --- a/autonav_ws/src/autonav_msgs/CMakeLists.txt +++ b/autonav_ws/src/autonav_msgs/CMakeLists.txt @@ -20,7 +20,6 @@ set(msg_files "msg/Position.msg" "msg/GoalPoint.msg" "msg/Waypoint.msg" - "msg/ObjectDetection.msg" "msg/Path.msg" "msg/Obstacle.msg" "msg/Obstacles.msg" diff --git a/autonav_ws/src/autonav_msgs/msg/ObjectDetection.msg b/autonav_ws/src/autonav_msgs/msg/ObjectDetection.msg deleted file mode 100644 index a25a0f67..00000000 --- a/autonav_ws/src/autonav_msgs/msg/ObjectDetection.msg +++ /dev/null @@ -1,3 +0,0 @@ -byte sensor_1 -byte sensor_2 -byte sensor_3 \ No newline at end of file diff --git a/autonav_ws/src/autonav_nav/CMakeLists.txt b/autonav_ws/src/autonav_nav/CMakeLists.txt index ae34a5e5..590c249a 100644 --- a/autonav_ws/src/autonav_nav/CMakeLists.txt +++ b/autonav_ws/src/autonav_nav/CMakeLists.txt @@ -16,7 +16,6 @@ ament_python_install_package(${PROJECT_NAME}) install(PROGRAMS src/path_resolver.py src/astar.py - src/pure_pursuit.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 76790f28..ac9bc533 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -24,26 +24,19 @@ HORIZONTAL_CAMERA_DIST = 3 CV_BRIDGE = cv_bridge.CvBridge() -# STARTING_PT = (42.6681268, -83.218887) - competition_waypoints = [ - # Start Nomans, Frist Ramp, Middle Ramp, End Ramp, End Nomans - [(42.6682837222, -83.2193403028), (42.6681206444, -83.2193606083), (42.66809863885, -83.2193606083), (42.6680766333, -83.2193606083), - (42.6679277056, -83.2193276417), (42.6679817056, -83.2193316417), (42.66794, -83.2189), (42.6681268, -83.218887)], # Start Facing North - # [(42.6679277056,-83.2193276417),(42.6680766333,-83.2193591583),(42.6681206444,-83.2193606083),(42.6682837222 ,-83.2193403028), (42.668290020, -83.2189080), (42.6681268, -83.218887)], # Start Facing South - # [(42.668222,-83.218472),(42.6680859611,-83.2184456444),(42.6679600583,-83.2184326556)], # Start Facing North - # [(42.6679600583,-83.2184326556), (42.6680859611,-83.2184456444),(42.668222,-83.218472)], # Start Facing South - [], + [(), (), (), ()], # NORTH + [(), (), (), ()] # SOUTH ] practice_waypoints = [ - [(35.2104852, -97.44193), (35.210483, -97.44207), (35.2104841, -97.4421986), (35.2104819, -97.4423302), (35.2105455, -97.4423329), - (35.2106096, -97.4423347), (35.2106107, -97.4422153), (35.2106078, -97.4421059), (35.2105575, -97.4420365), (35.2104852, -97.44193)] + [], # NORTH + [] # SOUTH ] simulation_waypoints = [ - [(35.19505, -97.43823),(35.19492, -97.43824),(35.19485, -97.43824),(35.19471, -97.43823)], # Facing North - [(35.19471, -97.43823),(35.19492, -97.43824),(35.19485, -97.43824),(35.19505, -97.43823)], # Facing South + [(35.19505, -97.43823), (35.19492, -97.43824), (35.19485, -97.43824), (35.19471, -97.43823)], # NORTH + [(35.19471, -97.43823), (35.19492, -97.43824), (35.19485, -97.43824), (35.19505, -97.43823)] # SOUTH ] @@ -90,17 +83,16 @@ def get_default_config(self): return AStarNodeConfig() def init(self): - self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.onConfigSpaceReceived, 20) - self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.onPoseReceived, 20) - self.imuSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.onImuReceived, 20) + self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.cfg_space_Received, 20) + self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.pose_received, 20) self.debugPublisher = self.create_publisher(PathingDebug, "/autonav/debug/astar", 20) self.pathPublisher = self.create_publisher(Path, "/autonav/path", 20) self.safetyLightsPublisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 20) self.pathDebugImagePublisher = self.create_publisher(CompressedImage, "/autonav/debug/astar/image", self.qos_profile) - self.mapTimer = self.create_timer(0.1, self.createPath) + self.map_timer = self.create_timer(0.1, self.create_path) - self.resetWhen = -1.0 - self.waypointTime = time.time() + self.config.waypointDelay + self.reset_at = -1.0 + self.waypoint_start_time = time.time() + self.config.waypointDelay self.set_device_state(DeviceStateEnum.OPERATING) def getAngleDifference(self, to_angle, from_angle): @@ -108,20 +100,16 @@ def getAngleDifference(self, to_angle, from_angle): delta = (delta + math.pi) % (2 * math.pi) - math.pi return delta - def onImuReceived(self, msg: IMUData): - self.imu = msg - def onReset(self): - self.imu = None - self.lastPath = None + self.last_path = None self.position = None - self.configSpace = None - self.costMap = None - self.bestPosition = (0, 0) + self.cfg_spce = None + self.cost_map = None + self.best_pos = (0, 0) self.waypoints = [] - self.waypointTime = self.config.waypointDelay + time.time() + self.waypoint_start_time = self.config.waypointDelay + time.time() - def getWaypointsForDirection(self): + def get_waypoints_for_dir(self): # Get out current heading and estimate within 180 degrees which direction we are facing (north or south, 0 and 1 respectively) heading = self.position.theta direction_index = 0 @@ -137,41 +125,41 @@ def getWaypointsForDirection(self): def system_state_transition(self, old: SystemState, updated: SystemState): if updated.state == SystemStateEnum.AUTONOMOUS and updated.mobility and len(self.waypoints) == 0: - self.waypointTime = time.time() + self.config.waypointDelay + self.waypoint_start_time = time.time() + self.config.waypointDelay if updated.state != SystemStateEnum.AUTONOMOUS and self.device_state == DeviceStateEnum.OPERATING: self.onReset() - def onPoseReceived(self, msg: Position): + def pose_received(self, msg: Position): self.position = msg - def createPath(self): - if self.position is None or self.costMap is None: + def create_path(self): + if self.position is None or self.cost_map is None: return robot_pos = (40, 78) - path = self.findPathToPoint(robot_pos, self.bestPosition, self.costMap, 80, 80) + path = self.find_path_to_point(robot_pos, self.best_pos, self.cost_map, 80, 80) if path is not None: global_path = Path() - global_path.poses = [self.pathToGlobalPose(pp[0], pp[1]) for pp in path] - self.lastPath = path + global_path.poses = [self.path_to_global(pp[0], pp[1]) for pp in path] + self.last_path = path self.pathPublisher.publish(global_path) # Draw the cost map onto a debug iamge cvimg = np.zeros((80, 80), dtype=np.uint8) for i in range(80): for j in range(80): - cvimg[i, j] = self.costMap[i * 80 + j] * 255 + cvimg[i, j] = self.cost_map[i * 80 + j] * 255 cvimg = cv2.cvtColor(cvimg, cv2.COLOR_GRAY2RGB) for pp in path: cv2.circle(cvimg, (pp[0], pp[1]), 1, (0, 255, 0), 1) - cv2.circle(cvimg, (self.bestPosition[0], self.bestPosition[1]), 1, (255, 0, 0), 1) - cvimg = cv2.resize(cvimg, (800, 800),interpolation=cv2.INTER_NEAREST) + cv2.circle(cvimg, (self.best_pos[0], self.best_pos[1]), 1, (255, 0, 0), 1) + cvimg = cv2.resize(cvimg, (800, 800), interpolation=cv2.INTER_NEAREST) self.pathDebugImagePublisher.publish(CV_BRIDGE.cv2_to_compressed_imgmsg(cvimg)) - def reconstructPath(self, path, current): + def reconstruct_path(self, path, current): total_path = [current] while current in path: @@ -180,7 +168,7 @@ def reconstructPath(self, path, current): return total_path[::-1] - def findPathToPoint(self, start, goal, map, width, height): + def find_path_to_point(self, start, goal, map, width, height): looked_at = np.zeros((80, 80)) open_set = [start] path = {} @@ -217,7 +205,7 @@ def getG(pt): looked_at[current[0], current[1]] = 1 if current == goal: - return self.reconstructPath(path, current) + return self.reconstruct_path(path, current) open_set.remove(current) for delta_x, delta_y, dist in search_dirs: @@ -235,7 +223,7 @@ def getG(pt): open_set.append(neighbor) heappush(next_current, (fScore[neighbor], neighbor)) - def onConfigSpaceReceived(self, msg: OccupancyGrid): + def cfg_space_Received(self, msg: OccupancyGrid): if self.position is None or self.system_state != SystemStateEnum.AUTONOMOUS: return @@ -250,31 +238,31 @@ def onConfigSpaceReceived(self, msg: OccupancyGrid): if self.config.useOnlyWaypoints == True: grid_data = [0] * len(msg.data) - if len(self.waypoints) == 0 and time.time() > self.waypointTime and self.waypointTime != 0: - self.waypoints = [wp for wp in self.getWaypointsForDirection()] - self.waypointTime = 0 + if len(self.waypoints) == 0 and time.time() > self.waypoint_start_time and self.waypoint_start_time != 0: + self.waypoints = [wp for wp in self.get_waypoints_for_dir()] + self.waypoint_start_time = 0 - if time.time() < self.waypointTime and len(self.waypoints) == 0: - time_remaining = self.waypointTime - time.time() + if time.time() < self.waypoint_start_time and len(self.waypoints) == 0: + time_remaining = self.waypoint_start_time - time.time() pathingDebug = PathingDebug() pathingDebug.waypoints = [] pathingDebug.time_until_use_waypoints = time_remaining self.debugPublisher.publish(pathingDebug) - if time.time() > self.resetWhen and self.resetWhen != -1 and self.mobility: + if time.time() > self.reset_at and self.reset_at != -1 and self.mobility: self.safetyLightsPublisher.publish(toSafetyLights(True, False, 2, 255, "#FFFFFF")) - self.resetWhen = -1 + self.reset_at = -1 if len(self.waypoints) > 0: next_waypoint = self.waypoints[0] north_to_gps = (next_waypoint[0] - self.position.latitude) * self.latitudeLength - west_to_gps = (self.position.longitude -next_waypoint[1]) * self.longitudeLength + west_to_gps = (self.position.longitude - next_waypoint[1]) * self.longitudeLength heading_to_gps = math.atan2(west_to_gps, north_to_gps) % (2 * math.pi) if north_to_gps ** 2 + west_to_gps ** 2 <= self.config.waypointPopDistance: self.waypoints.pop(0) self.safetyLightsPublisher.publish(toSafetyLights(True, False, 2, 255, "#00FF00")) - self.resetWhen = time.time() + 1.5 + self.reset_at = time.time() + 1.5 pathingDebug = PathingDebug() pathingDebug.desired_heading = heading_to_gps @@ -318,10 +306,10 @@ def onConfigSpaceReceived(self, msg: OccupancyGrid): depth += 1 - self.costMap = grid_data - self.bestPosition = temp_best_pos + self.cost_map = grid_data + self.best_pos = temp_best_pos - def pathToGlobalPose(self, pp0, pp1): + def path_to_global(self, pp0, pp1): x = (80 - pp1) * VERTICAL_CAMERA_DIST / 80 y = (40 - pp0) * HORIZONTAL_CAMERA_DIST / 80 diff --git a/autonav_ws/src/autonav_nav/src/path_resolver.py b/autonav_ws/src/autonav_nav/src/path_resolver.py index 7ff5f8fe..09de5c35 100644 --- a/autonav_ws/src/autonav_nav/src/path_resolver.py +++ b/autonav_ws/src/autonav_nav/src/path_resolver.py @@ -63,7 +63,7 @@ def init(self): self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 1) self.config = self.get_default_config() - self.create_timer(0.05, self.onResolve) + self.create_timer(0.05, self.resolve) self.set_device_state(DeviceStateEnum.READY) def config_updated(self, jsonObject): @@ -108,7 +108,7 @@ def on_path_received(self, msg: Path): self.points = [x.pose.position for x in msg.poses] self.pure_pursuit.set_points([(point.x, point.y)for point in self.points]) - def onResolve(self): + def resolve(self): if self.position is None or self.device_state != DeviceStateEnum.OPERATING or self.system_state != SystemStateEnum.AUTONOMOUS or not self.mobility: return diff --git a/autonav_ws/src/autonav_nav/src/pure_pursuit.py b/autonav_ws/src/autonav_nav/src/pure_pursuit.py deleted file mode 100644 index 0184250f..00000000 --- a/autonav_ws/src/autonav_nav/src/pure_pursuit.py +++ /dev/null @@ -1,65 +0,0 @@ -import math - -# Gracefully stolen from https://github.com/xiaoxiae/PurePursuitAlgorithm/blob/master/src/main/PurePursuit.java -class PurePursuit: - - def __init__(self): - self.path = [] - - def add_point(self, x, y): - self.path.append((x,y)) - - def set_points(self, pts): - self.path = pts - - def get_lookahead_point(self, x, y, r): - lookahead = None - - for i in range(len(self.path)-1): - segStart = self.path[i] - segEnd = self.path[i+1] - - p1 = (segStart[0] - x, segStart[1] - y) - p2 = (segEnd[0] - x, segEnd[1] - y) - - dx = p2[0] - p1[0] - dy = p2[1] - p1[1] - - d = math.sqrt(dx * dx + dy * dy) - D = p1[0] * p2[1] - p2[0] * p1[1] - - discriminant = r * r * d * d - D * D - if discriminant < 0 or p1 == p2: - continue - - sign = lambda x: (1, -1)[x < 0] - - x1 = (D * dy + sign(dy) * dx * math.sqrt(discriminant)) / (d * d) - x2 = (D * dy - sign(dy) * dx * math.sqrt(discriminant)) / (d * d) - - y1 = (-D * dx + abs(dy) * math.sqrt(discriminant)) / (d * d) - y2 = (-D * dx - abs(dy) * math.sqrt(discriminant)) / (d * d) - - validIntersection1 = min(p1[0], p2[0]) < x1 and x1 < max(p1[0], p2[0]) or min(p1[1], p2[1]) < y1 and y1 < max(p1[1], p2[1]) - validIntersection2 = min(p1[0], p2[0]) < x2 and x2 < max(p1[0], p2[0]) or min(p1[1], p2[1]) < y2 and y2 < max(p1[1], p2[1]) - - if validIntersection1 or validIntersection2: - lookahead = None - - if validIntersection1: - lookahead = (x1 + x, y1 + y) - - if validIntersection2: - if lookahead == None or abs(x1 - p2[0]) > abs(x2 - p2[0]) or abs(y1 - p2[1]) > abs(y2 - p2[1]): - lookahead = (x2 + x, y2 + y) - - if len(self.path) > 0: - lastPoint = self.path[len(self.path) - 1] - - endX = lastPoint[0] - endY = lastPoint[1] - - if math.sqrt((endX - x) * (endX - x) + (endY - y) * (endY - y)) <= r: - return (endX, endY) - - return lookahead \ No newline at end of file From 5e0ba734b8cc9259da4a2d4608d54c18243ad39e Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 14:39:45 -0500 Subject: [PATCH 048/113] Adjust default expandify max range --- autonav_ws/src/autonav_vision/src/expandify.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 696dffd6..03e3fce1 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -94,7 +94,7 @@ class ExpandifyNode : public SCR::Node newConfig.vertical_fov = 2.75; newConfig.horizontal_fov = 3.4; newConfig.map_res = 80.0f; - newConfig.max_range = 0.73; + newConfig.max_range = 1.0; newConfig.no_go_percent = 0.70; newConfig.no_go_range = 0; return newConfig; From 02d73f25b813273666050ff658d4f8d6dc90cb9b Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 14:40:09 -0500 Subject: [PATCH 049/113] Fix various issues with compilation --- autonav_ws/src/autonav_display/src/display.py | 2 +- autonav_ws/src/autonav_nav/CMakeLists.txt | 1 + .../src/autonav_nav/src/pure_pursuit.py | 65 +++++++++++++++++++ .../src/autonav_playback/src/playback.py | 2 +- .../src/autonav_serial/src/serial_node.py | 13 +--- 5 files changed, 69 insertions(+), 14 deletions(-) create mode 100644 autonav_ws/src/autonav_nav/src/pure_pursuit.py diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index f7ac51c0..3a3df8d3 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -7,7 +7,7 @@ from scr_msgs.msg import SystemState, DeviceState, ConfigUpdated from scr_msgs.srv import SetSystemState, UpdateConfig from std_msgs.msg import Empty -from autonav_msgs.msg import Position, MotorFeedback, MotorInput, MotorControllerDebug, ObjectDetection, PathingDebug, GPSFeedback, IMUData, Conbus +from autonav_msgs.msg import Position, MotorFeedback, MotorInput, MotorControllerDebug, PathingDebug, GPSFeedback, IMUData, Conbus from sensor_msgs.msg import CompressedImage import asyncio import websockets diff --git a/autonav_ws/src/autonav_nav/CMakeLists.txt b/autonav_ws/src/autonav_nav/CMakeLists.txt index 590c249a..3d3ac60f 100644 --- a/autonav_ws/src/autonav_nav/CMakeLists.txt +++ b/autonav_ws/src/autonav_nav/CMakeLists.txt @@ -14,6 +14,7 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS + src/pure_pursuit.py src/path_resolver.py src/astar.py DESTINATION lib/${PROJECT_NAME} diff --git a/autonav_ws/src/autonav_nav/src/pure_pursuit.py b/autonav_ws/src/autonav_nav/src/pure_pursuit.py new file mode 100644 index 00000000..0184250f --- /dev/null +++ b/autonav_ws/src/autonav_nav/src/pure_pursuit.py @@ -0,0 +1,65 @@ +import math + +# Gracefully stolen from https://github.com/xiaoxiae/PurePursuitAlgorithm/blob/master/src/main/PurePursuit.java +class PurePursuit: + + def __init__(self): + self.path = [] + + def add_point(self, x, y): + self.path.append((x,y)) + + def set_points(self, pts): + self.path = pts + + def get_lookahead_point(self, x, y, r): + lookahead = None + + for i in range(len(self.path)-1): + segStart = self.path[i] + segEnd = self.path[i+1] + + p1 = (segStart[0] - x, segStart[1] - y) + p2 = (segEnd[0] - x, segEnd[1] - y) + + dx = p2[0] - p1[0] + dy = p2[1] - p1[1] + + d = math.sqrt(dx * dx + dy * dy) + D = p1[0] * p2[1] - p2[0] * p1[1] + + discriminant = r * r * d * d - D * D + if discriminant < 0 or p1 == p2: + continue + + sign = lambda x: (1, -1)[x < 0] + + x1 = (D * dy + sign(dy) * dx * math.sqrt(discriminant)) / (d * d) + x2 = (D * dy - sign(dy) * dx * math.sqrt(discriminant)) / (d * d) + + y1 = (-D * dx + abs(dy) * math.sqrt(discriminant)) / (d * d) + y2 = (-D * dx - abs(dy) * math.sqrt(discriminant)) / (d * d) + + validIntersection1 = min(p1[0], p2[0]) < x1 and x1 < max(p1[0], p2[0]) or min(p1[1], p2[1]) < y1 and y1 < max(p1[1], p2[1]) + validIntersection2 = min(p1[0], p2[0]) < x2 and x2 < max(p1[0], p2[0]) or min(p1[1], p2[1]) < y2 and y2 < max(p1[1], p2[1]) + + if validIntersection1 or validIntersection2: + lookahead = None + + if validIntersection1: + lookahead = (x1 + x, y1 + y) + + if validIntersection2: + if lookahead == None or abs(x1 - p2[0]) > abs(x2 - p2[0]) or abs(y1 - p2[1]) > abs(y2 - p2[1]): + lookahead = (x2 + x, y2 + y) + + if len(self.path) > 0: + lastPoint = self.path[len(self.path) - 1] + + endX = lastPoint[0] + endY = lastPoint[1] + + if math.sqrt((endX - x) * (endX - x) + (endY - y) * (endY - y)) <= r: + return (endX, endY) + + return lookahead \ No newline at end of file diff --git a/autonav_ws/src/autonav_playback/src/playback.py b/autonav_ws/src/autonav_playback/src/playback.py index 558d872d..e945d48f 100644 --- a/autonav_ws/src/autonav_playback/src/playback.py +++ b/autonav_ws/src/autonav_playback/src/playback.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 from types import SimpleNamespace -from autonav_msgs.msg import IMUData, GPSFeedback, MotorFeedback, MotorInput, Position, MotorControllerDebug, ObjectDetection +from autonav_msgs.msg import IMUData, GPSFeedback, MotorFeedback, MotorInput, Position, MotorControllerDebug from scr.states import DeviceStateEnum, SystemStateEnum from sensor_msgs.msg import CompressedImage from scr_msgs.msg import SystemState, DeviceState diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index eeec2120..59d03808 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -6,7 +6,7 @@ import can import threading import struct -from autonav_msgs.msg import MotorInput, MotorFeedback, ObjectDetection, MotorControllerDebug, SafetyLights, Conbus +from autonav_msgs.msg import MotorInput, MotorFeedback, MotorControllerDebug, SafetyLights, Conbus from scr.node import Node from scr.states import DeviceStateEnum @@ -16,7 +16,6 @@ MOBILITY_STOP_ID = 1 MOBILITY_START_ID = 9 MOTOR_FEEDBACK_ID = 14 -OBJECT_DETECTION = 20 SAFETY_LIGHTS_ID = 13 CAN_50 = 50 @@ -51,7 +50,6 @@ def __init__(self): self.safetyLightsSubscriber = self.create_subscription(SafetyLights, "/autonav/SafetyLights", self.onSafetyLightsReceived, 20) self.motorInputSubscriber = self.create_subscription(MotorInput, "/autonav/MotorInput", self.onMotorInputReceived, 20) self.motorDebugPublisher = self.create_publisher(MotorControllerDebug, "/autonav/MotorControllerDebug", 20) - self.objectDetectionPublisher = self.create_publisher(ObjectDetection, "/autonav/ObjectDetection", 20) self.motorFeedbackPublisher = self.create_publisher(MotorFeedback, "/autonav/MotorFeedback", 20) self.conbuSubscriber = self.create_subscription(Conbus, "/autonav/conbus/instruction", self.onConbusReceived, 20) self.conbusPublisher = self.create_publisher(Conbus, "/autonav/conbus/data", 20) @@ -120,15 +118,6 @@ def onCanMessageReceived(self, msg): pkg.timestamp = self.getClockMs() * 1.0 self.motorDebugPublisher.publish(pkg) - if arb_id == OBJECT_DETECTION: - # Load in 4 bytes - zero, left, middle, right = struct.unpack("BBBB", msg.data) - pkg = ObjectDetection() - pkg.sensor_1 = left - pkg.sensor_2 = middle - pkg.sensor_3 = right - self.objectDetectionPublisher.publish(pkg) - if arb_id >= 1000 and arb_id < 1400: # self.log(f"[CAN -> {arb_id}] Received ConBus message") pkg = Conbus() From abc93d33641c4b59451aecbf168d333bb9446d1c Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 14:40:26 -0500 Subject: [PATCH 050/113] Adjust a* debug to always publish --- autonav_ws/src/autonav_nav/src/astar.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index ac9bc533..ada4e087 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -143,7 +143,9 @@ def create_path(self): global_path = Path() global_path.poses = [self.path_to_global(pp[0], pp[1]) for pp in path] self.last_path = path - self.pathPublisher.publish(global_path) + + if self.system_state == SystemStateEnum.AUTONOMOUS and self.mobility: + self.pathPublisher.publish(global_path) # Draw the cost map onto a debug iamge cvimg = np.zeros((80, 80), dtype=np.uint8) @@ -224,7 +226,7 @@ def getG(pt): heappush(next_current, (fScore[neighbor], neighbor)) def cfg_space_Received(self, msg: OccupancyGrid): - if self.position is None or self.system_state != SystemStateEnum.AUTONOMOUS: + if self.position is None: return grid_data = msg.data From 062f698af73ca52941142fb5230868905dff4dc0 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 15:11:37 -0500 Subject: [PATCH 051/113] More display tomfoolerly --- display/scripts/main.js | 99 +++++++++++++++------------------------- display/styles/index.css | 8 ++++ 2 files changed, 45 insertions(+), 62 deletions(-) diff --git a/display/scripts/main.js b/display/scripts/main.js index 673fbe77..409c4a15 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -701,119 +701,94 @@ $(document).ready(function () { return div; } else if (type == "parallelogram.int") { - // 4 boxes with 2 integers each const div = document.createElement("div"); - div.classList.add("input-group"); - div.classList.add("mb-3"); - - const inputX1 = document.createElement("input"); - inputX1.type = "number"; - inputX1.classList.add("form-control"); - inputX1.value = data[0][0]; - inputX1.onchange = function () { + div.classList.add("input-group", "mb-3"); + + function createCoordinateInput(value, onChangeHandler) { + const input = document.createElement("input"); + input.type = "number"; + input.classList.add("form-control", "coordinate-input"); + input.value = value; + input.onchange = onChangeHandler; + return input; + } + + const inputX1 = createCoordinateInput(data[0][0], function () { config[device][text][0][0] = parseInt(inputX1.value); send({ op: "configuration", device: device, json: config[device], }); - } - - const inputY1 = document.createElement("input"); - inputY1.type = "number"; - inputY1.classList.add("form-control"); - inputY1.value = data[0][1]; - inputY1.onchange = function () { + }); + + const inputY1 = createCoordinateInput(data[0][1], function () { config[device][text][0][1] = parseInt(inputY1.value); send({ op: "configuration", device: device, json: config[device], }); - } - - const inputX2 = document.createElement("input"); - inputX2.type = "number"; - inputX2.classList.add("form-control"); - inputX2.value = data[1][0]; - inputX2.onchange = function () { + }); + + const inputX2 = createCoordinateInput(data[1][0], function () { config[device][text][1][0] = parseInt(inputX2.value); send({ op: "configuration", device: device, json: config[device], }); - } - - const inputY2 = document.createElement("input"); - inputY2.type = "number"; - inputY2.classList.add("form-control"); - inputY2.value = data[1][1]; - inputY2.onchange = function () { + }); + + const inputY2 = createCoordinateInput(data[1][1], function () { config[device][text][1][1] = parseInt(inputY2.value); send({ op: "configuration", device: device, json: config[device], }); - } - - const inputX3 = document.createElement("input"); - inputX3.type = "number"; - inputX3.classList.add("form-control"); - inputX3.value = data[2][0]; - inputX3.onchange = function () { + }); + + const inputX3 = createCoordinateInput(data[2][0], function () { config[device][text][2][0] = parseInt(inputX3.value); send({ op: "configuration", device: device, json: config[device], }); - } - - const inputY3 = document.createElement("input"); - inputY3.type = "number"; - inputY3.classList.add("form-control"); - inputY3.value = data[2][1]; - inputY3.onchange = function () { + }); + + const inputY3 = createCoordinateInput(data[2][1], function () { config[device][text][2][1] = parseInt(inputY3.value); send({ op: "configuration", device: device, json: config[device], }); - } - - const inputX4 = document.createElement("input"); - inputX4.type = "number"; - inputX4.classList.add("form-control"); - inputX4.value = data[3][0]; - inputX4.onchange = function () { + }); + + const inputX4 = createCoordinateInput(data[3][0], function () { config[device][text][3][0] = parseInt(inputX4.value); send({ op: "configuration", device: device, json: config[device], }); - } - - const inputY4 = document.createElement("input"); - inputY4.type = "number"; - inputY4.classList.add("form-control"); - inputY4.value = data[3][1]; - inputY4.onchange = function () { + }); + + const inputY4 = createCoordinateInput(data[3][1], function () { config[device][text][3][1] = parseInt(inputY4.value); send({ op: "configuration", device: device, json: config[device], }); - } - + }); + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(inputX1); div.appendChild(inputY1); @@ -823,7 +798,7 @@ $(document).ready(function () { div.appendChild(inputY3); div.appendChild(inputX4); div.appendChild(inputY4); - return div; + return div; } else { const options = addressKeys[device][text]; diff --git a/display/styles/index.css b/display/styles/index.css index 89a40a7f..6bb7dc23 100644 --- a/display/styles/index.css +++ b/display/styles/index.css @@ -175,4 +175,12 @@ span[data-state="5"] { #logging { margin: 1rem; +} + +.coordinate-input { + width: 60px; + margin-right: 5px; + text-align: center; + border-radius: 5px; + border: 1px solid #ced4da; } \ No newline at end of file From fd9ba8ef719ddc9c99c9a83346e56431df6a4944 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 15:54:38 -0500 Subject: [PATCH 052/113] Try and fix combination image not showing up correctly --- autonav_ws/src/autonav_vision/src/combination.py | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 33af510d..47b01d54 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -93,12 +93,11 @@ def try_combine_grids(self): self.combined_grid_publisher.publish(combined_grid) # Publish the combined grid as an image - preview_image = np.zeros((IMAGE_HEIGHT, IMAGE_WIDTH), np.uint8) - for i in range(len(combined_grid.data)): - x = i % combined_grid.info.width - y = i // combined_grid.info.width - if combined_grid.data[i] > 0: - preview_image[y, x] = 255 + preview_image = np.zeros((80, 80), dtype=np.uint8) + for i in range(80): + for j in range(80): + preview_image[i, j] = 255 - combined_grid.data[i * 80 + j] * 255 / 100 + preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB) compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) self.combined_grid_image_publisher.publish(compressed_image) From 26179a163da2c81c6a8c07bea70a6b83cd4c3622 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 15:54:52 -0500 Subject: [PATCH 053/113] Update astar to use cfg for fov/waypoint weight --- autonav_ws/src/autonav_nav/src/astar.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index ada4e087..03e703cf 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -20,13 +20,11 @@ GRID_SIZE = 0.1 -VERTICAL_CAMERA_DIST = 2.75 -HORIZONTAL_CAMERA_DIST = 3 CV_BRIDGE = cv_bridge.CvBridge() competition_waypoints = [ - [(), (), (), ()], # NORTH - [(), (), (), ()] # SOUTH + [], # NORTH + [] # SOUTH ] practice_waypoints = [ @@ -65,6 +63,10 @@ def __init__(self): self.waypointDirection = 0 self.useOnlyWaypoints = False self.waypointDelay = 18 + self.waypointWeight = 1.0 + self.waypointMaxWeight = 10.0 + self.horizontal_fov = 3.4 + self.vertical_fov = 2.75 class AStarNode(Node): @@ -288,7 +290,7 @@ def cfg_space_Received(self, msg: OccupancyGrid): if len(self.waypoints) > 0: heading_err_to_gps = abs(self.getAngleDifference(self.position.theta + math.atan2(40 - x, 80 - y), heading_to_gps)) * 180 / math.pi - cost -= max(heading_err_to_gps, 10) + cost -= max(heading_err_to_gps, self.config.waypointMaxWeight) * self.config.waypointWeight if cost > best_pos_cost: best_pos_cost = cost @@ -312,8 +314,8 @@ def cfg_space_Received(self, msg: OccupancyGrid): self.best_pos = temp_best_pos def path_to_global(self, pp0, pp1): - x = (80 - pp1) * VERTICAL_CAMERA_DIST / 80 - y = (40 - pp0) * HORIZONTAL_CAMERA_DIST / 80 + x = (80 - pp1) * self.config.vertical_fov / 80 + y = (40 - pp0) * self.config.horizontal_fov / 80 new_x = x * math.cos(0) + y * math.sin(0) new_y = x * math.sin(0) + y * math.cos(0) From ddb94a6c9269196374465ee856caadbb58ecb3ee Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 17:56:52 -0500 Subject: [PATCH 054/113] Update camera output dimensions in camera.py and add new options for vision adjustment and visual lines in globals.js --- autonav_ws/src/autonav_serial/src/camera.py | 4 ++-- display/scripts/globals.js | 9 ++++++++- 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 9f65b17f..2691000b 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -18,8 +18,8 @@ class CameraNodeConfig: def __init__(self): self.refresh_rate = 8 - self.output_width = 640 - self.output_height = 480 + self.output_width = 480 + self.output_height = 640 self.camera_index_left = 0 self.camera_index_right = 2 self.scan_rate = 1.0 diff --git a/display/scripts/globals.js b/display/scripts/globals.js index fd0e2694..18357cf8 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -30,7 +30,14 @@ var addressKeys = { "output_width": "int", "output_height": "int", "camera_index_left": "int", - "camera_index_right": "int" + "camera_index_right": "int", + "right_flip_horizontal": "bool", + "right_flip_vertical": "bool", + "rotate_right_clockwise": "bool", + "left_flip_horizontal": "bool", + "left_flip_vertical": "bool", + "rotate_left_clockwise": "bool", + "scan_rate": "int", }, "autonav_vision_transformer": { From aee39a4415afc9814612765fa031e84bc93c9095 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:00:59 -0500 Subject: [PATCH 055/113] Update camera.py to destroy and create threads after config update --- autonav_ws/src/autonav_serial/src/camera.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 2691000b..848344b3 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -81,6 +81,9 @@ def destroy_threads(self): def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + self.destroy_threads() + self.create_threads() + def get_default_config(self): return CameraNodeConfig() From b5c3d2716e4f989577b7a90874d14d776c77567b Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:02:02 -0500 Subject: [PATCH 056/113] Fix issue with camera thread destruction in camera.py --- autonav_ws/src/autonav_serial/src/camera.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 848344b3..873bcf94 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -59,6 +59,9 @@ def create_threads(self): self.camera_thread_right.start() def destroy_threads(self): + if self.camera_thread_left is None or self.camera_thread_right is None: + return + self.lock_left.acquire() self.left_kill = True self.lock_left.release() From 59a7d929250c45d2d17730ca4266e0587c214eab Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:02:13 -0500 Subject: [PATCH 057/113] Refactor camera thread destruction and creation in camera.py --- autonav_ws/src/autonav_serial/src/camera.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 873bcf94..00994771 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -42,6 +42,9 @@ def __init__(self): self.left_kill = False self.right_kill = False + self.camera_thread_left = None + self.camera_thread_right = None + self.autofocus = 0 self.autoexposure = 0 From dd43881e7292dade60326e4a77bb5ed092ea8c2a Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:03:18 -0500 Subject: [PATCH 058/113] Refactor camera thread destruction and creation in camera.py --- autonav_ws/src/autonav_serial/src/camera.py | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 00994771..832411eb 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -37,8 +37,6 @@ def __init__(self): self.camera_publisher_left = self.create_publisher(CompressedImage, "/autonav/camera/compressed/left", 20) self.camera_publisher_right = self.create_publisher(CompressedImage, "/autonav/camera/compressed/right", 20) - self.lock_left = threading.Lock() - self.lock_right = threading.Lock() self.left_kill = False self.right_kill = False @@ -65,24 +63,14 @@ def destroy_threads(self): if self.camera_thread_left is None or self.camera_thread_right is None: return - self.lock_left.acquire() self.left_kill = True - self.lock_left.release() - - self.lock_right.acquire() self.right_kill = True - self.lock_right.release() self.camera_thread_left.join() self.camera_thread_right.join() - self.lock_left.acquire() self.left_kill = False - self.lock_left.release() - - self.lock_right.acquire() self.right_kill = False - self.lock_right.release() def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) From 83190119fb12ac6fc9738942b57968ba6b8d465c Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:03:48 -0500 Subject: [PATCH 059/113] Update camera.py to change the camera index for the right camera --- autonav_ws/src/autonav_serial/src/camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 832411eb..4bb5bc45 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -21,7 +21,7 @@ def __init__(self): self.output_width = 480 self.output_height = 640 self.camera_index_left = 0 - self.camera_index_right = 2 + self.camera_index_right = 1 self.scan_rate = 1.0 self.left_flip_horizontal = True From 697cb4e6381bac784d02b299c42f24e578fb45be Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:05:36 -0500 Subject: [PATCH 060/113] Update camera.py to change the camera index for the right camera --- autonav_ws/src/autonav_serial/src/camera.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 4bb5bc45..832411eb 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -21,7 +21,7 @@ def __init__(self): self.output_width = 480 self.output_height = 640 self.camera_index_left = 0 - self.camera_index_right = 1 + self.camera_index_right = 2 self.scan_rate = 1.0 self.left_flip_horizontal = True From 5b74dd0e474ce846b413b0fa48280cb932edd4a2 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:10:22 -0500 Subject: [PATCH 061/113] Update camera.py to change the camera index for the left and right cameras --- autonav_ws/src/autonav_serial/src/camera.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 832411eb..91a67d86 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -20,8 +20,8 @@ def __init__(self): self.refresh_rate = 8 self.output_width = 480 self.output_height = 640 - self.camera_index_left = 0 - self.camera_index_right = 2 + self.camera_index_left = 1 + self.camera_index_right = 3 self.scan_rate = 1.0 self.left_flip_horizontal = True From 56fd827818f7dacd5cfb2b78bbd49c9e21654239 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:12:22 -0500 Subject: [PATCH 062/113] Update camera.py to change the camera index for the left and right cameras --- autonav_ws/src/autonav_serial/src/camera.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 91a67d86..832411eb 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -20,8 +20,8 @@ def __init__(self): self.refresh_rate = 8 self.output_width = 480 self.output_height = 640 - self.camera_index_left = 1 - self.camera_index_right = 3 + self.camera_index_left = 0 + self.camera_index_right = 2 self.scan_rate = 1.0 self.left_flip_horizontal = True From 75554b5df5361f01cefb6d9b44cc86bdfdd4ddef Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:14:04 -0500 Subject: [PATCH 063/113] Revert to old camera.py --- autonav_ws/src/autonav_serial/src/camera.py | 84 +++++---------------- 1 file changed, 18 insertions(+), 66 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 832411eb..e1670008 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -18,34 +18,19 @@ class CameraNodeConfig: def __init__(self): self.refresh_rate = 8 - self.output_width = 480 - self.output_height = 640 + self.output_width = 640 + self.output_height = 480 self.camera_index_left = 0 self.camera_index_right = 2 self.scan_rate = 1.0 - self.left_flip_horizontal = True - self.left_flip_vertical = True - self.right_flip_horizontal = True - self.right_flip_vertical = True - self.rotate_left_clockwise = True - self.rotate_right_clockwise = True class CameraNode(Node): def __init__(self): super().__init__("autonav_serial_camera") self.camera_publisher_left = self.create_publisher(CompressedImage, "/autonav/camera/compressed/left", 20) self.camera_publisher_right = self.create_publisher(CompressedImage, "/autonav/camera/compressed/right", 20) - self.left_kill = False - self.right_kill = False - - self.camera_thread_left = None - self.camera_thread_right = None - - self.autofocus = 0 - self.autoexposure = 0 - def init(self): self.get_logger().info("Initializing camera node...") self.create_threads() @@ -59,57 +44,18 @@ def create_threads(self): self.camera_thread_right.daemon = True self.camera_thread_right.start() - def destroy_threads(self): - if self.camera_thread_left is None or self.camera_thread_right is None: - return - - self.left_kill = True - self.right_kill = True - - self.camera_thread_left.join() - self.camera_thread_right.join() - - self.left_kill = False - self.right_kill = False - def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) - self.destroy_threads() - self.create_threads() - def get_default_config(self): return CameraNodeConfig() - - def apply_transformations(self, frame, dir): - if dir == "left": - if self.config.left_flip_horizontal: - frame = cv2.flip(frame, 1) - if self.config.left_flip_vertical: - frame = cv2.flip(frame, 0) - if self.config.rotate_left_clockwise: - frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) - else: - frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) - else: - if self.config.right_flip_horizontal: - frame = cv2.flip(frame, 1) - if self.config.right_flip_vertical: - frame = cv2.flip(frame, 0) - if self.config.rotate_right_clockwise: - frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) - else: - frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) - - frame.resize(self.config.output_width, self.config.output_height) - return frame def camera_worker(self, *args, **kwargs): index_name = args[0] if len(args) > 0 else "" camera_index = self.config.camera_index_left if index_name == "left" else self.config.camera_index_right capture = None - while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN and (index_name == "left" and not self.left_kill or index_name == "right" and not self.right_kill): + while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: try: if not os.path.exists("/dev/video" + str(camera_index)): time.sleep(self.config.scan_rate) @@ -122,22 +68,29 @@ def camera_worker(self, *args, **kwargs): capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.config.output_width) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.config.output_height) - capture.set(cv2.CAP_PROP_AUTOFOCUS, self.autofocus) - capture.set(cv2.CAP_PROP_AUTO_EXPOSURE, self.autoexposure) + # self.set_device_state(DeviceStateEnum.OPERATING) except: + # self.set_device_state(DeviceStateEnum.STANDBY) time.sleep(self.config.scan_rate) continue while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: + # if self.device_state != DeviceStateEnum.OPERATING: + # continue + try: ret, frame = capture.read() - self.apply_transformations(frame, index_name) + if index_name == "left": + frame = cv2.flip(frame, 1) + frame = cv2.flip(frame, 0) + frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) + frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) except: - if capture is None: - break + if capture is not None: + capture.release() + capture = None - capture.release() - capture = None + # self.set_device_state(DeviceStateEnum.STANDBY) break if not ret or frame is None: @@ -147,7 +100,6 @@ def camera_worker(self, *args, **kwargs): self.camera_publisher_left.publish(bridge.cv2_to_compressed_imgmsg(frame)) else: self.camera_publisher_right.publish(bridge.cv2_to_compressed_imgmsg(frame)) - time.sleep(1.0 / self.config.refresh_rate) @@ -159,4 +111,4 @@ def main(): if __name__ == "__main__": - main() + main() \ No newline at end of file From 1c479ab4a3a655a7bffd97777851f8572cf789ee Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:16:23 -0500 Subject: [PATCH 064/113] Update camera.py to change the output dimensions for the left and right cameras --- autonav_ws/src/autonav_serial/src/camera.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index e1670008..28d8cde5 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -18,8 +18,8 @@ class CameraNodeConfig: def __init__(self): self.refresh_rate = 8 - self.output_width = 640 - self.output_height = 480 + self.output_width = 480 + self.output_height = 640 self.camera_index_left = 0 self.camera_index_right = 2 self.scan_rate = 1.0 @@ -83,6 +83,7 @@ def camera_worker(self, *args, **kwargs): if index_name == "left": frame = cv2.flip(frame, 1) frame = cv2.flip(frame, 0) + frame = cv2.flip(frame, 0) frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) except: From 29ccc7b080d4de30e658f3b53f429a936fa965c0 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 14 May 2024 18:19:34 -0500 Subject: [PATCH 065/113] Fix camera flipping issue in camera.py --- autonav_ws/src/autonav_serial/src/camera.py | 1 - 1 file changed, 1 deletion(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 28d8cde5..2b92df75 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -83,7 +83,6 @@ def camera_worker(self, *args, **kwargs): if index_name == "left": frame = cv2.flip(frame, 1) frame = cv2.flip(frame, 0) - frame = cv2.flip(frame, 0) frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) except: From e0e57ec32e36ecf53dc9671b7613bfbbb14c6245 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Sun, 19 May 2024 21:44:00 -0500 Subject: [PATCH 066/113] Switch to using a node per camera --- autonav_ws/src/autonav_serial/src/camera.py | 88 +++++++++++---------- display/scripts/globals.js | 26 +++--- 2 files changed, 64 insertions(+), 50 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 2b92df75..de781ab2 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -20,93 +20,101 @@ def __init__(self): self.refresh_rate = 8 self.output_width = 480 self.output_height = 640 - self.camera_index_left = 0 - self.camera_index_right = 2 self.scan_rate = 1.0 + self.flip_horizontal = False + self.flip_vertical = False + self.rotate_clockwise = False class CameraNode(Node): - def __init__(self): - super().__init__("autonav_serial_camera") - self.camera_publisher_left = self.create_publisher(CompressedImage, "/autonav/camera/compressed/left", 20) - self.camera_publisher_right = self.create_publisher(CompressedImage, "/autonav/camera/compressed/right", 20) + def __init__(self, side, udev_path): + super().__init__("autonav_serial_camera_" + side) + self.camera_publisher = self.create_publisher(CompressedImage, "/autonav/camera/compressed/" + side, self.qos_profile) + self.camera_thread = None + self.camera_kill = False + self.camera_side = side + self.camera_path = udev_path def init(self): - self.get_logger().info("Initializing camera node...") - self.create_threads() - - def create_threads(self): - self.camera_thread_left = threading.Thread(target=self.camera_worker, args=("left",)) - self.camera_thread_left.daemon = True - self.camera_thread_left.start() - - self.camera_thread_right = threading.Thread(target=self.camera_worker, args=("right",)) - self.camera_thread_right.daemon = True - self.camera_thread_right.start() + self.create_thread() + + def create_thread(self): + if self.camera_thread is not None: + self.camera_kill = True + self.camera_thread.join() + self.camera_thread = None + + self.camera_kill = False + self.camera_thread = threading.Thread(target=self.camera_worker) + self.camera_thread.start() def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + self.create_thread() def get_default_config(self): return CameraNodeConfig() - def camera_worker(self, *args, **kwargs): - index_name = args[0] if len(args) > 0 else "" - camera_index = self.config.camera_index_left if index_name == "left" else self.config.camera_index_right - + def camera_worker(self): capture = None - while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: + while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN and not self.camera_kill: try: - if not os.path.exists("/dev/video" + str(camera_index)): + if not os.path.exists(self.camera_path): time.sleep(self.config.scan_rate) continue - capture = cv2.VideoCapture(camera_index) + capture = cv2.VideoCapture(self.camera_path) if capture is None or not capture.isOpened(): time.sleep(self.config.scan_rate) continue capture.set(cv2.CAP_PROP_FRAME_WIDTH, self.config.output_width) capture.set(cv2.CAP_PROP_FRAME_HEIGHT, self.config.output_height) - # self.set_device_state(DeviceStateEnum.OPERATING) + self.set_device_state(DeviceStateEnum.OPERATING) except: - # self.set_device_state(DeviceStateEnum.STANDBY) + self.set_device_state(DeviceStateEnum.STANDBY) time.sleep(self.config.scan_rate) continue - while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: - # if self.device_state != DeviceStateEnum.OPERATING: - # continue + while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN and not self.camera_kill: + if self.device_state != DeviceStateEnum.OPERATING: + continue try: ret, frame = capture.read() - if index_name == "left": + + if self.config.rotate_clockwise: + frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) + else: + frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) + + frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) + + if self.config.flip_horizontal: frame = cv2.flip(frame, 1) + + if self.config.flip_vertical: frame = cv2.flip(frame, 0) - frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) - frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) except: if capture is not None: capture.release() capture = None - # self.set_device_state(DeviceStateEnum.STANDBY) + self.set_device_state(DeviceStateEnum.STANDBY) break if not ret or frame is None: continue - - if index_name == "left": - self.camera_publisher_left.publish(bridge.cv2_to_compressed_imgmsg(frame)) - else: - self.camera_publisher_right.publish(bridge.cv2_to_compressed_imgmsg(frame)) + + self.camera_publisher.publish(bridge.cv2_to_compressed_imgmsg(frame)) time.sleep(1.0 / self.config.refresh_rate) def main(): rclpy.init() - node = CameraNode() - Node.run_node(node) + node_left = CameraNode("left", "/dev/autonav_camera_left") + node_right = CameraNode("right", "/dev/autonav_camera_right") + Node.run_nodes([node_left, node_right]) rclpy.shutdown() diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 18357cf8..85e34bbf 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -24,20 +24,26 @@ var addressKeys = { "imu_read_rate": "float" }, - "autonav_serial_camera": { - "internal_title": "[Serial] Camera", + "autonav_serial_camera_left": { + "internal_title": "[Serial] Left Camera", "refresh_rate": "int", "output_width": "int", "output_height": "int", - "camera_index_left": "int", - "camera_index_right": "int", - "right_flip_horizontal": "bool", - "right_flip_vertical": "bool", - "rotate_right_clockwise": "bool", - "left_flip_horizontal": "bool", - "left_flip_vertical": "bool", - "rotate_left_clockwise": "bool", "scan_rate": "int", + "flip_horizontal": "bool", + "flip_vertical": "bool", + "rotate_clockwise": "bool" + }, + + "autonav_serial_camera_right": { + "internal_title": "[Serial] Right Camera", + "refresh_rate": "int", + "output_width": "int", + "output_height": "int", + "scan_rate": "int", + "flip_horizontal": "bool", + "flip_vertical": "bool", + "rotate_clockwise": "bool" }, "autonav_vision_transformer": { From 8b37095071c20a8b061f88925334bf68c97327ef Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Sun, 19 May 2024 22:50:37 -0500 Subject: [PATCH 067/113] Add shutdown functionality to node.py and node.cpp --- autonav_ws/src/scr/scr/node.py | 7 +++++++ autonav_ws/src/scr/src/node.cpp | 12 ++++++------ 2 files changed, 13 insertions(+), 6 deletions(-) diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index 2b64d248..c76f88d5 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -11,6 +11,8 @@ import rclpy from rclpy.executors import MultiThreadedExecutor import json +import os +import signal class Node(ROSNode): @@ -122,6 +124,11 @@ def on_system_state(self, msg: SystemState): :param msg: The system state message. """ + + # If the system state is shutdown, kill this node killing the proces + if msg.state == SystemStateEnum.SHUTDOWN: + os.kill(os.getpid(), signal.SIGINT) + return oldState = SystemState() oldState.state = self.system_state diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index dfab8e05..41551a8e 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -56,6 +56,12 @@ namespace SCR oldState.mobility = mobility; oldState.mode = system_mode; + // If the new system state is shutdown, just exit the process + if (msg.state == static_cast(SCR::SystemState::SHUTDOWN)) + { + kill(getpid(), SIGINT); + } + SCR::SystemState newStateEnum = static_cast(msg.state); SCR::SystemState oldStateEnum = static_cast(oldState.state); @@ -67,12 +73,6 @@ namespace SCR system_mode = static_cast(msg.mode); mobility = msg.mobility; system_state = static_cast(msg.state); - - if (system_state == SCR::SystemState::SHUTDOWN) - { - // Just exit this process and die - exit(0); - } } void Node::device_state_callback(const scr_msgs::msg::DeviceState msg) From ad6b215c25c3284b239bbe313a8f95903ad2daed Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 16:50:47 -0500 Subject: [PATCH 068/113] The preset system should now work? --- autonav_ws/src/autonav_display/src/display.py | 821 ++++++++---------- .../src/autonav_playback/src/playback.py | 11 +- autonav_ws/src/scr/include/scr/constants.hpp | 4 + autonav_ws/src/scr/scr/constants.py | 6 +- autonav_ws/src/scr/scr/node.py | 27 +- autonav_ws/src/scr/src/node.cpp | 4 +- autonav_ws/src/scr_controller/src/core.cpp | 248 +++++- autonav_ws/src/scr_msgs/CMakeLists.txt | 4 + autonav_ws/src/scr_msgs/srv/DeletePreset.srv | 3 + autonav_ws/src/scr_msgs/srv/GetPresets.srv | 4 + .../src/scr_msgs/srv/SaveActivePreset.srv | 4 + .../src/scr_msgs/srv/SetActivePreset.srv | 3 + display/index.html | 73 +- display/scripts/globals.js | 30 +- display/scripts/main.js | 172 ++-- display/styles/index.css | 60 +- 16 files changed, 896 insertions(+), 578 deletions(-) create mode 100644 autonav_ws/src/scr_msgs/srv/DeletePreset.srv create mode 100644 autonav_ws/src/scr_msgs/srv/GetPresets.srv create mode 100644 autonav_ws/src/scr_msgs/srv/SaveActivePreset.srv create mode 100644 autonav_ws/src/scr_msgs/srv/SetActivePreset.srv diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 3a3df8d3..43883158 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -3,9 +3,8 @@ import rclpy from scr.node import Node from scr.states import DeviceStateEnum -# from scr_msgs.msg import SystemState, DeviceState, Log, ConfigurationInstruction from scr_msgs.msg import SystemState, DeviceState, ConfigUpdated -from scr_msgs.srv import SetSystemState, UpdateConfig +from scr_msgs.srv import SetSystemState, UpdateConfig, SetActivePreset, SaveActivePreset, GetPresets, DeletePreset from std_msgs.msg import Empty from autonav_msgs.msg import Position, MotorFeedback, MotorInput, MotorControllerDebug, PathingDebug, GPSFeedback, IMUData, Conbus from sensor_msgs.msg import CompressedImage @@ -16,479 +15,365 @@ import base64 import time -big_loop = asyncio.new_event_loop() +async_loop = asyncio.new_event_loop() + class Limiter: - def __init__(self) -> None: - self.limits = {} - self.nextAllowance = {} - - # Sets a limit for how many times per second a key can be used - def setLimit(self, key, limit): - self.limits[key] = limit - self.nextAllowance[key] = 0 - - # If it can be used, returns true and decrements the remaining uses - def use(self, key): - if key not in self.limits: - return True - - nextUsageAfter = self.nextAllowance[key] - if nextUsageAfter == 0: - self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) - return True - - if time.time() >= nextUsageAfter: - self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) - return True - - return False + def __init__(self) -> None: + self.limits = {} + self.nextAllowance = {} + + # Sets a limit for how many times per second a key can be used + def setLimit(self, key, limit): + self.limits[key] = limit + self.nextAllowance[key] = 0 + + # If it can be used, returns true and decrements the remaining uses + def use(self, key): + if key not in self.limits: + return True + + nextUsageAfter = self.nextAllowance[key] + if nextUsageAfter == 0: + self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) + return True + + if time.time() >= nextUsageAfter: + self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) + return True + + return False + class BroadcastNode(Node): - def __init__(self): - super().__init__("autonav_display_broadcast") - - self.port = 8023 - self.host = "0.0.0.0" - self.sendMap = {} - self.socketMap = {} - - self.limiter = Limiter() - self.limiter.setLimit("/autonav/MotorInput", 5) - self.limiter.setLimit("/autonav/MotorFeedback", 5) - self.limiter.setLimit("/autonav/MotorControllerDebug", 1) - self.limiter.setLimit("/autonav/imu", 5) - self.limiter.setLimit("/autonav/gps", 5) - self.limiter.setLimit("/autonav/position", 5) - self.limiter.setLimit("/autonav/camera/compressed/left", 2) - self.limiter.setLimit("/autonav/camera/compressed/right", 2) - self.limiter.setLimit("/autonav/cfg_space/raw/image/left", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/image/left_small", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/image/right_small", 5) - self.limiter.setLimit("/autonav/cfg_space/combined/image", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) - self.limiter.setLimit("/autonav/debug/astar/image", 5) - self.limiter.setLimit("/autonav/debug/astar", 5) - - self.get_logger().info("Broadcasting on ws://{}:{}".format(self.host, self.port)) - - self.systemStateSubscriber = self.create_subscription(SystemState, "/scr/system_state", self.systemStateCallback, 20) - self.deviceStateSubscriber = self.create_subscription(DeviceState, "/scr/device_state", self.deviceStateCallback, 20) - self.broadcastPublisher = self.create_publisher(Empty, "/scr/state/broadcast", 20) - self.configurationInstructionSubscriber = self.create_subscription(ConfigUpdated, "/scr/config_updated", self.configurationInstructionCallback, 100) - # self.logSubscriber = self.create_subscription(Log, "/scr/logging", self.logCallback, 20) - # self.configurationInstructionPublisher = self.create_publisher(ConfigUpdated, "/scr/config_updated", 100) - self.configUpdateClient = self.create_client(UpdateConfig, "/scr/update_config_client") - - self.positionSubscriber = self.create_subscription(Position, "/autonav/position", self.positionCallback, 20) - self.motorFeedbackSubscriber = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.motorFeedbackCallback, 20) - self.motorInputSubscriber = self.create_subscription(MotorInput, "/autonav/MotorInput", self.motorInputCallback, 20) - self.motorControllerDebugSubscriber = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.motorControllerDebugCallback, 20) - self.pathingDebugSubscriber = self.create_subscription(PathingDebug, "/autonav/debug/astar", self.pathingDebugCallback, 20) - self.gpsFeedbackSubscriber = self.create_subscription(GPSFeedback, "/autonav/gps", self.gpsFeedbackCallback, 20) - self.imuDataSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.imuDataCallback, 20) - self.conbusSubscriber = self.create_subscription(Conbus, "/autonav/conbus/data", self.conbusCallback, 100) - self.conbusPublisher = self.create_publisher(Conbus, "/autonav/conbus/instruction", 100) - - self.systemStateService = self.create_client(SetSystemState, "/scr/state/set_system_state") - - self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left/cutout", self.cameraCallbackLeft, self.qos_profile) - self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right/cutout", self.cameraCallbackRight, self.qos_profile) - self.filteredSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, self.qos_profile) - self.filteredSubscriberRight = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, self.qos_profile) - self.filteredSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left_small", self.filteredCallbackLeftSmall, self.qos_profile) - self.filteredSubscriberRight = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right_small", self.filteredCallbackRightSmall, self.qos_profile) - self.filteredSubscriberCombined = self.create_subscription(CompressedImage, "/autonav/cfg_space/combined/image", self.filteredCallbackCombined, self.qos_profile) - self.bigboiSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.bigboiCallback, self.qos_profile) - self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, self.qos_profile) - - self.get_logger().info("Starting event loop") - - self.loop_thread = threading.Thread(target=self.loopthread) - self.loop_thread.start() - - def loopthread(self): - asyncio.set_event_loop(big_loop) - big_loop.run_until_complete(self.start()) - - async def start(self): - async with websockets.serve(self.handler, self.host, self.port): - self.get_logger().info("Started websocket server") - await asyncio.Future() - - def getUserIdFromSocket(self, websocket): - try: - return websocket.path.split("?")[1].split("=")[1] - except: - return None - - def pushSendQueue(self, message, unique_id=None): - if len(self.sendMap) == 0: - return - - if unique_id is None: - for unique_id in self.sendMap: - self.sendMap[unique_id].append(message) - else: - self.sendMap[unique_id].append(message) - - async def producer(self, websocket): - unqiue_id = self.getUserIdFromSocket(websocket) - while True: - if len(self.sendMap[unqiue_id]) > 0: - await websocket.send(self.sendMap[unqiue_id].pop(0)) - else: - await asyncio.sleep(0.01) - - async def consumer(self, websocket): - self.get_logger().info("New websocket connection") - unique_id = self.getUserIdFromSocket(websocket) - async for message in websocket: - obj = json.loads(message) - if obj["op"] == "broadcast": - self.broadcastPublisher.publish(Empty()) - - if obj["op"] == "configuration" and "device" in obj and "json" in obj: - self.log("Received configuration instruction for " + obj["device"]) - config_packet = UpdateConfig.Request() - config_packet.device = obj["device"] - config_packet.json = json.dumps(obj["json"]) - self.configUpdateClient.call_async(config_packet) - - if obj["op"] == "get_nodes": - nodes = self.get_node_names() - for i in range(len(nodes)): - nodes[i] = nodes[i].replace("/", "") - node_states = {} - for identifier in nodes: - node_states[identifier] = self.device_states[identifier] if identifier in self.device_states else 0 - self.pushSendQueue(json.dumps({ - "op": "get_nodes_callback", - "nodes": nodes, - "states": node_states, - "configs": self.node_configs, - "system": { - "state": self.system_state, - "mode": self.system_mode, - "mobility": self.mobility - } - }), unique_id) - - if obj["op"] == "set_system_state": - self.set_system_total_state(int(obj["state"]), self.system_mode, bool(obj["mobility"])) - - if obj["op"] == "conbus": - id = int(obj["id"]) - data = list(map(lambda x: int(x), obj["data"])) - msg = Conbus() - msg.id = id - msg.data = data - msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0 - self.conbusPublisher.publish(msg) - - async def handler(self, websocket): - unique_id = self.getUserIdFromSocket(websocket) - if unique_id in self.socketMap or unique_id is None: - await websocket.close() - return - - self.socketMap[unique_id] = websocket - self.sendMap[unique_id] = [] - - consumer_task = asyncio.create_task(self.consumer(websocket)) - producer_task = asyncio.create_task(self.producer(websocket)) - pending = await asyncio.wait( - [consumer_task, producer_task], - return_when=asyncio.FIRST_COMPLETED - ) - for task in pending: - for t in task: - t.cancel() - - del self.socketMap[unique_id] - del self.sendMap[unique_id] - - def systemStateCallback(self, msg: SystemState): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/scr/state/system", - "state": msg.state, - "estop": msg.estop, - "mobility": msg.mobility, - "mode": msg.mode - })) - - def deviceStateCallback(self, msg: DeviceState): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/scr/state/device", - "state": msg.state, - "device": msg.device - })) - - # def logCallback(self, msg: Log): - # if msg.node == "autonav_display_broadcast": - # return - # self.pushSendQueue(json.dumps({ - # "op": "data", - # "topic": "/scr/logging", - # "data": msg.data, - # "node": msg.node - # })) - - def configurationInstructionCallback(self, msg: ConfigUpdated): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/scr/configuration", - "data": msg.json - })) - - def positionCallback(self, msg: Position): - if not self.limiter.use("/autonav/position"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/position", - "x": msg.x, - "y": msg.y, - "theta": msg.theta, - "latitude": msg.latitude, - "longitude": msg.longitude - })) - - def motorInputCallback(self, msg: MotorInput): - if not self.limiter.use("/autonav/MotorInput"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/MotorInput", - "angular_velocity": msg.angular_velocity, - "forward_velocity": msg.forward_velocity - })) - - def motorFeedbackCallback(self, msg: MotorFeedback): - if not self.limiter.use("/autonav/MotorFeedback"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/MotorFeedback", - "delta_x": msg.delta_x, - "delta_y": msg.delta_y, - "delta_theta": msg.delta_theta - })) - - def imuDataCallback(self, msg: IMUData): - if not self.limiter.use("/autonav/imu"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/imu", - "accel_x": msg.accel_x, - "accel_y": msg.accel_y, - "accel_z": msg.accel_z, - "angular_x": msg.angular_x, - "angular_y": msg.angular_y, - "angular_z": msg.angular_z, - "yaw": msg.yaw, - "pitch": msg.pitch, - "roll": msg.roll - })) - - def gpsFeedbackCallback(self, msg: GPSFeedback): - if not self.limiter.use("/autonav/gps"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/gps", - "latitude": msg.latitude, - "longitude": msg.longitude, - "altitude": msg.altitude, - "satellites": msg.satellites, - "is_locked": msg.is_locked, - "gps_fix": msg.gps_fix - })) - - def pathingDebugCallback(self, msg: PathingDebug): - if not self.limiter.use("/autonav/debug/astar"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/debug/astar", - "desired_heading": msg.desired_heading, - "desired_latitude": msg.desired_latitude, - "desired_longitude": msg.desired_longitude, - "distance_to_destination": msg.distance_to_destination, - "waypoints": msg.waypoints.tolist(), - "time_until_use_waypoints": msg.time_until_use_waypoints, - })) - - def motorControllerDebugCallback(self, msg: MotorControllerDebug): - if not self.limiter.use("/autonav/MotorControllerDebug"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/MotorControllerDebug", - "current_forward_velocity": msg.current_forward_velocity, - "forward_velocity_setpoint": msg.forward_velocity_setpoint, - "current_angular_velocity": msg.current_angular_velocity, - "angular_velocity_setpoint": msg.angular_velocity_setpoint, - "left_motor_output": msg.left_motor_output, - "right_motor_output": msg.right_motor_output - })) - - def cameraCallbackLeft(self, msg: CompressedImage): - if not self.limiter.use("/autonav/camera/compressed/left"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/camera/compressed/left", - "format": msg.format, - "data": base64_str - })) - - def cameraCallbackRight(self, msg: CompressedImage): - if not self.limiter.use("/autonav/camera/compressed/right"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/camera/compressed/right", - "format": msg.format, - "data": base64_str - })) - - def filteredCallbackLeft(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image/left"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/image/left", - "format": msg.format, - "data": base64_str - })) - - def filteredCallbackRight(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image/right"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/image/right", - "format": msg.format, - "data": base64_str - })) - - def filteredCallbackLeftSmall(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image/left_small"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/image/left_small", - "format": msg.format, - "data": base64_str - })) - - def filteredCallbackRightSmall(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image/right_small"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/image/right_small", - "format": msg.format, - "data": base64_str - })) - - def filteredCallbackCombined(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/combined/image"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/combined/image", - "format": msg.format, - "data": base64_str - })) - - def bigboiCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/debug"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/debug", - "format": msg.format, - "data": base64_str - })) - - def debugAStarCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/debug/astar/image"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/debug/astar/image", - "format": msg.format, - "data": base64_str - })) - - def conbusCallback(self, msg: Conbus): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/conbus", - "id": msg.id, - "data": msg.data.tolist(), - "iterator": msg.iterator - })) - - def init(self): - self.set_device_state(DeviceStateEnum.OPERATING) + def __init__(self): + super().__init__("autonav_display_broadcast") + + self.port = 8023 + self.host = "0.0.0.0" + self.send_map = {} + self.client_map = {} + + # Limiter + self.limiter = Limiter() + self.limiter.setLimit("/autonav/MotorInput", 5) + self.limiter.setLimit("/autonav/MotorFeedback", 5) + self.limiter.setLimit("/autonav/MotorControllerDebug", 1) + self.limiter.setLimit("/autonav/imu", 5) + self.limiter.setLimit("/autonav/gps", 5) + self.limiter.setLimit("/autonav/position", 5) + self.limiter.setLimit("/autonav/camera/compressed/left", 2) + self.limiter.setLimit("/autonav/camera/compressed/right", 2) + self.limiter.setLimit("/autonav/cfg_space/raw/image/left", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/left_small", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/right_small", 5) + self.limiter.setLimit("/autonav/cfg_space/combined/image", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) + self.limiter.setLimit("/autonav/debug/astar/image", 5) + self.limiter.setLimit("/autonav/debug/astar", 5) + + # Clients + self.system_state_c = self.create_subscription(SystemState, "/scr/system_state", self.systemStateCallback, 20) + self.system_state_c = self.create_client(SetSystemState, "/scr/state/set_system_state") + self.config_c = self.create_client(UpdateConfig, "/scr/update_config_client") + self.get_presets_c = self.create_client(GetPresets, "/scr/get_presets") + self.set_active_preset_c = self.create_client(SetActivePreset, "/scr/set_active_preset") + self.save_active_preset_c = self.create_client(SaveActivePreset, "/scr/save_active_preset") + self.delete_preset_c = self.create_client(DeletePreset, "/scr/delete_preset") + + # Publishers + self.conbus_p = self.create_publisher(Conbus, "/autonav/conbus/instruction", 100) + self.broadcast_p = self.create_publisher(Empty, "/scr/state/broadcast", 20) + + # Subscriptions + self.device_state_s = self.create_subscription(DeviceState, "/scr/device_state", self.deviceStateCallback, 20) + self.config_s = self.create_subscription(ConfigUpdated, "/scr/config_updated", self.configurationInstructionCallback, 10) + self.position_s = self.create_subscription(Position, "/autonav/position", self.positionCallback, 20) + self.motor_feedback_s = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.motorFeedbackCallback, 20) + self.motor_input_s = self.create_subscription(MotorInput, "/autonav/MotorInput", self.motorInputCallback, 20) + self.motor_debug_s = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.motorControllerDebugCallback, 20) + self.pathing_s = self.create_subscription(PathingDebug, "/autonav/debug/astar", self.pathingDebugCallback, 20) + self.gps_s = self.create_subscription(GPSFeedback, "/autonav/gps", self.gpsFeedbackCallback, 20) + self.imu_s = self.create_subscription(IMUData, "/autonav/imu", self.imuDataCallback, 20) + self.conbus_s = self.create_subscription(Conbus, "/autonav/conbus/data", self.conbusCallback, 100) + self.camera_left_s = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left/cutout", self.cameraCallbackLeft, self.qos_profile) + self.camera_right_s = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right/cutout", self.cameraCallbackRight, self.qos_profile) + self.filtered_left_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left_small", self.filteredCallbackLeftSmall, self.qos_profile) + self.filtered_right_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right_small", self.filteredCallbackRightSmall, self.qos_profile) + self.combined_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/combined/image", self.filteredCallbackCombined, self.qos_profile) + self.inflated_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.inflated_callback, self.qos_profile) + self.astar_debug_s = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, self.qos_profile) + + self.loop_thread = threading.Thread(target=self.loopthread) + self.loop_thread.start() + + def loopthread(self): + asyncio.set_event_loop(async_loop) + async_loop.run_until_complete(self.start()) + + async def start(self): + async with websockets.serve(self.handler, self.host, self.port): + self.get_logger().info("Started websocket server") + await asyncio.Future() + + def get_id_from_socket(self, websocket): + try: + return websocket.path.split("?")[1].split("=")[1] + except: + return None + + def push_image(self, topic, msg): + if not self.limiter.use(topic): + return + + byts = msg.data.tobytes() + base64_str = base64.b64encode(byts).decode("utf-8") + + self.push_old(json.dumps({ + "op": "data", + "topic": topic, + "format": msg.format, + "data": base64_str + })) + + def push(self, topic, data, unique_id=None): + # Check limiter + if not self.limiter.use(topic): + return + + # Create packet + packet = { + "op": "data", + "topic": topic, + } + + # Copy properties from data to packet + for key in data.get_fields_and_field_types().keys(): + packet[key] = getattr(data, key) + + # Check if there are any clients + if len(self.send_map) == 0: + return + + # Convert to json + message_json = json.dumps(packet) + + # Send it out as needed + if unique_id is None: + for unique_id in self.send_map: + self.send_map[unique_id].append(message_json) + else: + self.send_map[unique_id].append(message_json) + + def push_old(self, message, unique_id=None): + if len(self.send_map) == 0: + return + + if unique_id is None: + for unique_id in self.send_map: + self.send_map[unique_id].append(message) + else: + self.send_map[unique_id].append(message) + + async def producer(self, websocket): + unqiue_id = self.get_id_from_socket(websocket) + while True: + if len(self.send_map[unqiue_id]) > 0: + await websocket.send(self.send_map[unqiue_id].pop(0)) + else: + await asyncio.sleep(0.01) + + async def consumer(self, websocket): + unique_id = self.get_id_from_socket(websocket) + async for message in websocket: + obj = json.loads(message) + if obj["op"] == "broadcast": + self.broadcast_p.publish(Empty()) + + if obj["op"] == "configuration" and "device" in obj and "json" in obj: + config_packet = UpdateConfig.Request() + config_packet.device = obj["device"] + config_packet.json = json.dumps(obj["json"]) + self.config_c.call_async(config_packet) + + if obj["op"] == "get_nodes": + nodes = self.get_node_names() + for i in range(len(nodes)): + nodes[i] = nodes[i].replace("/", "") + node_states = {} + for identifier in nodes: + node_states[identifier] = self.device_states[identifier] if identifier in self.device_states else 0 + self.push_old(json.dumps({ + "op": "get_nodes_callback", + "nodes": nodes, + "states": node_states, + "configs": self.node_configs, + "system": { + "state": self.system_state, + "mode": self.system_mode, + "mobility": self.mobility + } + }), unique_id) + + if obj["op"] == "set_system_state": + self.set_system_total_state(int(obj["state"]), self.system_mode, bool(obj["mobility"])) + + if obj["op"] == "conbus": + id = int(obj["id"]) + data = list(map(lambda x: int(x), obj["data"])) + msg = Conbus() + msg.id = id + msg.data = data + msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0 + self.conbus_p.publish(msg) + + if obj["op"] == "get_presets": + req = GetPresets.Request() + req.empty = True + res = self.get_presets_c.call_async(req) + res.add_done_callback(self.get_presets_callback) + + if obj["op"] == "set_active_preset": + req = SetActivePreset.Request() + req.preset = obj["preset"] + self.set_active_preset_c.call_async(req) + + if obj["op"] == "save_preset_mode": + req = SaveActivePreset.Request() + req.write_mode = True + req.preset_name = "" + self.save_active_preset_c.call_async(req) + + if obj["op"] == "save_preset_as": + req = SaveActivePreset.Request() + req.preset_name = obj["preset"] + req.write_mode = False + self.save_active_preset_c.call_async(req) + + if obj["op"] == "delete_preset": + req = DeletePreset.Request() + req.preset = obj["preset"] + self.delete_preset_c.call_async(req) + + def get_presets_callback(self, future): + response = future.result() + self.push_old(json.dumps({ + "op": "get_presets_callback", + "presets": response.presets, + "active_preset": response.active_preset + })) + + async def handler(self, websocket): + unique_id = self.get_id_from_socket(websocket) + if unique_id in self.client_map or unique_id is None: + await websocket.close() + return + + self.client_map[unique_id] = websocket + self.send_map[unique_id] = [] + + consumer_task = asyncio.create_task(self.consumer(websocket)) + producer_task = asyncio.create_task(self.producer(websocket)) + pending = await asyncio.wait( + [consumer_task, producer_task], + return_when=asyncio.FIRST_COMPLETED + ) + for task in pending: + for t in task: + t.cancel() + + del self.client_map[unique_id] + del self.send_map[unique_id] + + def systemStateCallback(self, msg: SystemState): + self.push("/scr/state/system", msg) + + def deviceStateCallback(self, msg: DeviceState): + self.push("/scr/state/device", msg) + + def configurationInstructionCallback(self, msg: ConfigUpdated): + self.push("/scr/configuration", msg) + + def positionCallback(self, msg: Position): + self.push("/autonav/position", msg) + + def motorInputCallback(self, msg: MotorInput): + self.push("/autonav/MotorInput", msg) + + def motorFeedbackCallback(self, msg: MotorFeedback): + self.push("/autonav/MotorFeedback", msg) + + def imuDataCallback(self, msg: IMUData): + self.push("/autonav/imu", msg) + + def gpsFeedbackCallback(self, msg: GPSFeedback): + self.push("/autonav/gps", msg) + + def pathingDebugCallback(self, msg: PathingDebug): + if not self.limiter.use("/autonav/debug/astar"): + return + + self.push_old(json.dumps({ + "op": "data", + "topic": "/autonav/debug/astar", + "desired_heading": msg.desired_heading, + "desired_latitude": msg.desired_latitude, + "desired_longitude": msg.desired_longitude, + "distance_to_destination": msg.distance_to_destination, + "waypoints": msg.waypoints.tolist(), + "time_until_use_waypoints": msg.time_until_use_waypoints, + })) + + def motorControllerDebugCallback(self, msg: MotorControllerDebug): + self.push("/autonav/MotorControllerDebug", msg) + + def cameraCallbackLeft(self, msg: CompressedImage): + self.push_image("/autonav/camera/compressed/left", msg) + + def cameraCallbackRight(self, msg: CompressedImage): + self.push_image("/autonav/camera/compressed/right", msg) + def filteredCallbackLeft(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/left", msg) + + def filteredCallbackRight(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/right", msg) + + def filteredCallbackLeftSmall(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/left_small", msg) + + def filteredCallbackRightSmall(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/right_small", msg) + + def filteredCallbackCombined(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/combined/image", msg) + + def inflated_callback(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/debug", msg) + + def debugAStarCallback(self, msg: CompressedImage): + self.push_image("/autonav/debug/astar/image", msg) + + def conbusCallback(self, msg: Conbus): + self.push_old(json.dumps({ + "op": "data", + "topic": "/autonav/conbus", + "id": msg.id, + "data": msg.data.tolist(), + "iterator": msg.iterator + })) + + def init(self): + self.set_device_state(DeviceStateEnum.OPERATING) def main(): - rclpy.init() - node = BroadcastNode() - Node.run_node(node) - rclpy.shutdown() + rclpy.init() + node = BroadcastNode() + Node.run_node(node) + rclpy.shutdown() + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/autonav_ws/src/autonav_playback/src/playback.py b/autonav_ws/src/autonav_playback/src/playback.py index e945d48f..e128a2fb 100644 --- a/autonav_ws/src/autonav_playback/src/playback.py +++ b/autonav_ws/src/autonav_playback/src/playback.py @@ -13,6 +13,7 @@ import cv2 import os import json +import subprocess class ImageTransformerConfig: @@ -64,6 +65,7 @@ def init(self): def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + self.get_logger().info(f"Updated config: {self.jdump(jsonObject)}") def get_default_config(self): return ImageTransformerConfig() @@ -80,7 +82,8 @@ def create_file(self): def create_video(self, folder, name): IMAGES_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name, "images", folder) SAVE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name) - os.system(f"ffmpeg -r {self.config.frame_rate} -i {IMAGES_PATH}/%d.jpg -vcodec libx264 -crf 18 -pix_fmt yuv420p -y {SAVE_PATH}/{name}.mp4") + with open(os.devnull, "wb") as devnull: + subprocess.call(["ffmpeg", "-r", f"{self.config.frame_rate}", "-i", f"{IMAGES_PATH}/%d.jpg", "-vcodec", "libx264", "-crf", "18", "-pix_fmt", "yuv420p", "-y", f"{SAVE_PATH}/{name}.mp4"], stdout=devnull, stderr=devnull) def create_entry(self): self.file_name = self.create_file() @@ -99,7 +102,7 @@ def create_entry(self): self.astar_index = 0 self.camera_index = 0 - self.log(f"Recording playback data at {BASE_PATH}") + self.get_logger().info(f"Recording playback data at {BASE_PATH}") def close_entry(self): if self.file is None: @@ -129,9 +132,10 @@ def close_entry(self): shutil.make_archive(BASE_PATH, "zip", BASE_PATH) SIZE_OF_ZIP = os.path.getsize(BASE_PATH + ".zip") / 1024 / 1024 TIME_ELAPSED = datetime.now().timestamp() - self.startTime - self.log(f"Saved playback data to {self.file_name}.zip ({SIZE_OF_ZIP:.3f} MB) over {TIME_ELAPSED:.3f} seconds") shutil.rmtree(BASE_PATH, ignore_errors=True) + self.get_logger().info(f"Finished recording playback data at {BASE_PATH}. Size of zip: {SIZE_OF_ZIP:.2f} MB. Time elapsed: {TIME_ELAPSED:.2f} seconds") + def write_file(self, msg: str): if self.file is None: return @@ -147,6 +151,7 @@ def write_image(self, img: CompressedImage, relative_path: str, idx: int): cv2.imwrite(os.path.join(IMAGE_PATH, f"{idx}.jpg"), cv2Image) def system_state_transition(self, old: SystemState, updated: SystemState): + self.get_logger().info(f"System state transitioned from {old.state} to {updated.state}") if old.state == SystemStateEnum.AUTONOMOUS and updated.state != SystemStateEnum.AUTONOMOUS: self.close_entry() diff --git a/autonav_ws/src/scr/include/scr/constants.hpp b/autonav_ws/src/scr/include/scr/constants.hpp index 34f17769..28cf15ec 100644 --- a/autonav_ws/src/scr/include/scr/constants.hpp +++ b/autonav_ws/src/scr/include/scr/constants.hpp @@ -19,6 +19,10 @@ namespace SCR const std::string SYSTEM_STATE = "/scr/system_state_client"; const std::string DEVICE_STATE = "/scr/device_state_client"; const std::string CONFIG_UPDATE = "/scr/update_config_client"; + const std::string SET_ACTIVE_PRESET = "/scr/set_active_preset"; + const std::string SAVE_ACTIVE_PRESET = "/scr/save_active_preset"; + const std::string GET_PRESETS = "/scr/get_presets"; + const std::string DELETE_PRESET = "/scr/delete_preset"; } } diff --git a/autonav_ws/src/scr/scr/constants.py b/autonav_ws/src/scr/scr/constants.py index ab66edaf..a20d3a2a 100644 --- a/autonav_ws/src/scr/scr/constants.py +++ b/autonav_ws/src/scr/scr/constants.py @@ -8,4 +8,8 @@ class Topics: class Services: SYSTEM_STATE = "/scr/system_state_client" DEVICE_STATE = "/scr/device_state_client" - CONFIG_UPDATE = "/scr/update_config_client" \ No newline at end of file + CONFIG_UPDATE = "/scr/update_config_client" + SET_ACTIVE_PRESET = "/scr/set_active_preset" + SAVE_ACTIVE_PRESET = "/scr/save_active_preset" + GET_PRESETS = "/scr/get_presets" + DELETE_PRESET = "/scr/delete_preset" \ No newline at end of file diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index c76f88d5..8eb0cd23 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -24,9 +24,9 @@ def __init__(self, node_name): super().__init__(node_name) self.identifier = node_name self.qos_profile = QoSProfile( - reliability = QoSReliabilityPolicy.BEST_EFFORT, - history = QoSHistoryPolicy.KEEP_LAST, - depth = 1 + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1 ) # Create the callback groups @@ -38,11 +38,11 @@ def __init__(self, node_name): self.device_state_subscriber = self.create_subscription(DeviceState, scr.constants.Topics.DEVICE_STATE, self.on_device_state, 10) self.system_state_subscriber = self.create_subscription(SystemState, scr.constants.Topics.SYSTEM_STATE, self.on_system_state, 10) self.config_updated_subscriber = self.create_subscription(ConfigUpdated, scr.constants.Topics.CONFIG_UPDATE, self.on_config_updated, 10) - + self.device_state_client = self.create_client(UpdateDeviceState, scr.constants.Services.DEVICE_STATE, callback_group=self.device_state_callback_group) self.system_state_client = self.create_client(UpdateSystemState, scr.constants.Services.SYSTEM_STATE, callback_group=self.system_state_callback_group) self.config_updated_client = self.create_client(UpdateConfig, scr.constants.Services.CONFIG_UPDATE, callback_group=self.config_updated_callback_group) - + self.performance_publisher = self.create_publisher(Float64, scr.constants.Topics.PERFORMANCE_TRACK, 10) self.logging_publisher = self.create_publisher(Log, scr.constants.Topics.LOGGING, 10) @@ -69,7 +69,7 @@ def jdump(self, obj): return json.dumps(obj.__dict__) else: return json.dumps(obj) - + def log(self, data): """ Logs a message to the logging topic. @@ -105,7 +105,7 @@ def on_device_state(self, msg: DeviceState): if not rclpy.ok(): self.get_logger().error("Interrupted while waiting for service") return - + try: result = self.config_updated_client.call(request) if not result.success: @@ -124,7 +124,7 @@ def on_system_state(self, msg: SystemState): :param msg: The system state message. """ - + # If the system state is shutdown, kill this node killing the proces if msg.state == SystemStateEnum.SHUTDOWN: os.kill(os.getpid(), signal.SIGINT) @@ -134,18 +134,17 @@ def on_system_state(self, msg: SystemState): oldState.state = self.system_state oldState.mode = self.system_mode oldState.mobility = self.mobility - - self.system_state_transition(oldState, msg) - self.system_state = msg.state self.system_mode = msg.mode self.mobility = msg.mobility + self.system_state_transition(oldState, msg) + def on_config_updated(self, msg: ConfigUpdated): self.node_configs[msg.device] = json.loads(msg.json) if msg.device is None or msg.device != self.identifier: return - + try: parsed_json = json.loads(msg.json) self.config_updated(parsed_json) @@ -259,7 +258,7 @@ def set_system_mobility(self, mobility: bool): self.set_system_total_state( self.system_state, self.system_mode, mobility) - + def perf_start(self, name: str): """ Starts a performance measurement. @@ -308,4 +307,4 @@ def run_nodes(nodes): executor.add_node(node) executor.spin() for node in nodes: - executor.remove_node(node) \ No newline at end of file + executor.remove_node(node) diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index 41551a8e..a643cfaa 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -68,11 +68,11 @@ namespace SCR SCR::SystemMode newMode = static_cast(msg.mode); SCR::SystemMode oldMode = static_cast(oldState.mode); - system_state_transition(oldState, msg); - system_mode = static_cast(msg.mode); mobility = msg.mobility; system_state = static_cast(msg.state); + + system_state_transition(oldState, msg); } void Node::device_state_callback(const scr_msgs::msg::DeviceState msg) diff --git a/autonav_ws/src/scr_controller/src/core.cpp b/autonav_ws/src/scr_controller/src/core.cpp index dbc343d7..913229bc 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -8,7 +8,14 @@ #include "scr/structs.hpp" #include "scr_msgs/srv/update_device_state.hpp" #include "scr_msgs/srv/update_config.hpp" +#include "scr_msgs/srv/set_active_preset.hpp" +#include "scr_msgs/srv/save_active_preset.hpp" +#include "scr_msgs/srv/get_presets.hpp" +#include "scr_msgs/srv/delete_preset.hpp" #include +#include +#include +#include #include "scr/json.hpp" using json = nlohmann::json; @@ -25,6 +32,10 @@ struct services rclcpp::Service::SharedPtr system_state; rclcpp::Service::SharedPtr device_state; rclcpp::Service::SharedPtr config_update; + rclcpp::Service::SharedPtr set_active_preset; + rclcpp::Service::SharedPtr save_active_preset; + rclcpp::Service::SharedPtr get_presets; + rclcpp::Service::SharedPtr delete_preset; }; class CoreNode : public rclcpp::Node @@ -39,17 +50,45 @@ class CoreNode : public rclcpp::Node services.system_state = this->create_service(SCR::Constants::Services::SYSTEM_STATE, std::bind(&CoreNode::on_system_state_called, this, std::placeholders::_1, std::placeholders::_2)); services.device_state = this->create_service(SCR::Constants::Services::DEVICE_STATE, std::bind(&CoreNode::on_device_state_called, this, std::placeholders::_1, std::placeholders::_2)); services.config_update = this->create_service(SCR::Constants::Services::CONFIG_UPDATE, std::bind(&CoreNode::on_config_update_called, this, std::placeholders::_1, std::placeholders::_2)); + services.set_active_preset = this->create_service(SCR::Constants::Services::SET_ACTIVE_PRESET, std::bind(&CoreNode::on_set_active_preset_called, this, std::placeholders::_1, std::placeholders::_2)); + services.save_active_preset = this->create_service(SCR::Constants::Services::SAVE_ACTIVE_PRESET, std::bind(&CoreNode::on_save_active_preset_called, this, std::placeholders::_1, std::placeholders::_2)); + services.get_presets = this->create_service(SCR::Constants::Services::GET_PRESETS, std::bind(&CoreNode::on_get_presets_called, this, std::placeholders::_1, std::placeholders::_2)); + services.delete_preset = this->create_service(SCR::Constants::Services::DELETE_PRESET, std::bind(&CoreNode::on_delete_preset_called, this, std::placeholders::_1, std::placeholders::_2)); // Load the initial system state from the parameters mode = static_cast(this->declare_parameter("mode", static_cast(SCR::SystemMode::COMPETITION))); state = static_cast(this->declare_parameter("state", static_cast(SCR::SystemState::DISABLED))); mobility = this->declare_parameter("mobility", false); + + // Set mode timer + mode_timer = this->create_wall_timer(std::chrono::seconds(2), std::bind(&CoreNode::mode_timer_callback, this)); } private: + std::string get_preset_by_name(std::string name) + { + // Look at $HOME/.config/autonav/{name}.preset + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + name + ".preset"; + if (!std::filesystem::exists(path)) + { + return ""; + } + + std::ifstream file(path); + std::string preset((std::istreambuf_iterator(file)), std::istreambuf_iterator()); + return preset; + } + + std::string get_preset_for_mode() + { + std::string modeStr = SCR::systemModeToString(mode); + return get_preset_by_name(modeStr); + } + /// @brief Callback for when the system state service is called. This is used to set the system state. - /// @param request - /// @param response + /// @param request + /// @param response void on_system_state_called(const std::shared_ptr request, std::shared_ptr response) { if (request->state < 0 || request->state > (uint8_t)SCR::SystemState::SHUTDOWN) @@ -83,8 +122,8 @@ class CoreNode : public rclcpp::Node } /// @brief Callback for when the device state service is called. This is used to set a specific devices state. - /// @param request - /// @param response + /// @param request + /// @param response void on_device_state_called(const std::shared_ptr request, std::shared_ptr response) { if (request->state < 0 || request->state > (uint8_t)SCR::DeviceState::ERRORED) @@ -121,6 +160,12 @@ class CoreNode : public rclcpp::Node config_updated_message.json = config.second.dump(); publishers.config_updated->publish(config_updated_message); } + + // Reset the mode timer + if (mode_timer_running) + { + mode_timer->reset(); + } } device_states[request->device] = (SCR::DeviceState)request->state; @@ -134,6 +179,16 @@ class CoreNode : public rclcpp::Node publishers.device_state->publish(device_state_message); } + void ensure_directories() + { + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/"; + if (!std::filesystem::exists(path)) + { + std::filesystem::create_directories(path); + } + } + void on_config_update_called(const std::shared_ptr request, std::shared_ptr response) { json config; @@ -161,6 +216,186 @@ class CoreNode : public rclcpp::Node publishers.config_updated->publish(config_updated_message); } + void on_set_active_preset_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + std::string preset = get_preset_by_name(request->preset); + if (preset.empty()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to load preset %s", request->preset.c_str()); + response->ok = false; + return; + } + + active_preset = json::parse(preset); + active_preset_name = request->preset; + for (auto const &[device, cfg] : active_preset) + { + if (configs.find(device) == configs.end()) + { + RCLCPP_ERROR(this->get_logger(), "Warning when loading preset %s: missing config for device %s", request->preset.c_str(), device.c_str()); + continue; + } + + // Update the config + configs[device] = cfg; + + // Publish the new config + scr_msgs::msg::ConfigUpdated config_updated_message; + config_updated_message.device = device; + config_updated_message.json = cfg.dump(); + publishers.config_updated->publish(config_updated_message); + } + + response->ok = true; + } + + void on_save_active_preset_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + active_preset = configs; + active_preset_name = request->write_mode ? SCR::systemModeToString(mode) : request->preset_name; + + std::string name = request->write_mode ? SCR::systemModeToString(mode) : request->preset_name; + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + name + ".preset"; + + // Write the preset to disk, if it exists then overwrite it + std::string jsonStr = nlohmann::json(active_preset).dump(); + std::ofstream file(path); + file << jsonStr; + file.close(); + + response->ok = true; + } + + void on_get_presets_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/"; + + // Get all files that end with .preset + presets.clear(); + for (const auto &entry : std::filesystem::directory_iterator(path)) + { + std::string filename = entry.path().filename().string(); + if (filename.find(".preset") != std::string::npos) + { + presets.push_back(filename.substr(0, filename.find(".preset"))); + } + } + + response->presets = presets; + response->active_preset = active_preset_name; + } + + void mode_timer_callback() + { + ensure_directories(); + + // Stop the mode timer + mode_timer_running = false; + mode_timer->cancel(); + mode_timer = nullptr; + + // Check if there is a preset for the current mode + std::string preset = get_preset_for_mode(); + if (!preset.empty()) + { + active_preset = json::parse(preset); + active_preset_name = SCR::systemModeToString(mode); + for (auto const &[device, cfg] : active_preset) + { + if (configs.find(device) == configs.end()) + { + continue; + } + + // Update the config + configs[device] = cfg; + + // Publish the new config + scr_msgs::msg::ConfigUpdated config_updated_message; + config_updated_message.device = device; + config_updated_message.json = cfg.dump(); + publishers.config_updated->publish(config_updated_message); + } + return; + } + + // If there is no preset for the current mode, then we need to create one + active_preset = configs; + active_preset_name = SCR::systemModeToString(mode); + std::string modeStr = SCR::systemModeToString(mode); + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + modeStr + ".preset"; + + // Write the preset to disk, if it exists then overwrite it + std::string jsonStr = nlohmann::json(active_preset).dump(); + std::ofstream file(path); + file << jsonStr; + file.close(); + + RCLCPP_INFO(this->get_logger(), "Created preset for mode %d", (int)mode); + } + + void on_delete_preset_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + + // Check that the preset exists + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + request->preset + ".preset"; + if (!std::filesystem::exists(path)) + { + response->ok = false; + return; + } + + // Ensure its not the active mode preset + if (request->preset == SCR::systemModeToString(mode)) + { + response->ok = false; + return; + } + + // Delete the preset + std::filesystem::remove(path); + + // Remove it from the list of presets + auto it = std::find(presets.begin(), presets.end(), request->preset); + if (it != presets.end()) + { + presets.erase(it); + } + + // If it was the active preset, change the active preset to the current system mode + if (active_preset_name == request->preset) + { + active_preset = json::parse(get_preset_for_mode()); + active_preset_name = SCR::systemModeToString(mode); + for (auto const &[device, cfg] : active_preset) + { + if (configs.find(device) == configs.end()) + { + continue; + } + + // Update the config + configs[device] = cfg; + + // Publish the new config + scr_msgs::msg::ConfigUpdated config_updated_message; + config_updated_message.device = device; + config_updated_message.json = cfg.dump(); + publishers.config_updated->publish(config_updated_message); + } + } + + response->ok = true; + } + private: struct publishers publishers; struct services services; @@ -170,6 +405,11 @@ class CoreNode : public rclcpp::Node bool mobility; std::map device_states; std::map configs; + std::map active_preset; + std::vector presets; + rclcpp::TimerBase::SharedPtr mode_timer; + bool mode_timer_running = true; + std::string active_preset_name; }; int main(int argc, char **argv) diff --git a/autonav_ws/src/scr_msgs/CMakeLists.txt b/autonav_ws/src/scr_msgs/CMakeLists.txt index 2c4a14de..5983d199 100644 --- a/autonav_ws/src/scr_msgs/CMakeLists.txt +++ b/autonav_ws/src/scr_msgs/CMakeLists.txt @@ -27,6 +27,10 @@ set(srv_files "srv/UpdateDeviceState.srv" "srv/UpdateConfig.srv" "srv/UpdateSystemState.srv" + "srv/SaveActivePreset.srv" + "srv/SetActivePreset.srv" + "srv/GetPresets.srv" + "srv/DeletePreset.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/autonav_ws/src/scr_msgs/srv/DeletePreset.srv b/autonav_ws/src/scr_msgs/srv/DeletePreset.srv new file mode 100644 index 00000000..b72a0a4b --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/DeletePreset.srv @@ -0,0 +1,3 @@ +string preset +--- +bool ok \ No newline at end of file diff --git a/autonav_ws/src/scr_msgs/srv/GetPresets.srv b/autonav_ws/src/scr_msgs/srv/GetPresets.srv new file mode 100644 index 00000000..a435d7aa --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/GetPresets.srv @@ -0,0 +1,4 @@ +bool empty +--- +string[] presets +string active_preset \ No newline at end of file diff --git a/autonav_ws/src/scr_msgs/srv/SaveActivePreset.srv b/autonav_ws/src/scr_msgs/srv/SaveActivePreset.srv new file mode 100644 index 00000000..e55a076a --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/SaveActivePreset.srv @@ -0,0 +1,4 @@ +string preset_name +bool write_mode +--- +bool ok \ No newline at end of file diff --git a/autonav_ws/src/scr_msgs/srv/SetActivePreset.srv b/autonav_ws/src/scr_msgs/srv/SetActivePreset.srv new file mode 100644 index 00000000..b72a0a4b --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/SetActivePreset.srv @@ -0,0 +1,3 @@ +string preset +--- +bool ok \ No newline at end of file diff --git a/display/index.html b/display/index.html index 1fc8e800..07099d77 100644 --- a/display/index.html +++ b/display/index.html @@ -21,21 +21,21 @@
  • Dashboard
    • -
  • + +
  • Configuration
  • Conbus
    • -
  • +
  • Debug
    • @@ -104,6 +104,25 @@
    Distance to Waypoint:
    Waypoints:
    +
    +
    +

    Vision

    +
    +
    +
    + + +
    +
    + + +
    +
    + + +
    +
    +

    Device States

    @@ -116,23 +135,37 @@

    Device States

    -
    -
    -

    Camera / Filtered

    +
    +
    +

    + Active Preset: None +

    +
    + + +
    +
    + + +
    +
    + + +
    -
    - - - - - - - - +
    -
    +
    -
    +
    diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 85e34bbf..e3fddb4a 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -16,7 +16,8 @@ var deviceStates = {}; var logs = []; var iterator = 0; var iterators = []; -var debug = false; +var development_mode = false; +var current_preset = "ERROR_NO_PRESET_AUTODETECTED"; var addressKeys = { "autonav_serial_imu": { @@ -75,7 +76,7 @@ var addressKeys = { "autonav_vision_expandifier": { "internal_title": "[Vision] Expandifier", - "horizontal_fov": "int", + "horizontal_fov": "float", "map_res": "int", "vertical_fov": "float", "max_range": "float", @@ -88,9 +89,7 @@ var addressKeys = { "filter_type": { 0: "Deadreckoning", 1: "Particle Filter" - }, - "degree_offset": "float", - "seed_heading": "bool", + } }, "autonav_manual_steamcontroller": { @@ -121,6 +120,10 @@ var addressKeys = { }, "useOnlyWaypoints": "bool", "waypointDelay": "float", + "vertical_fov": "float", + "horizontal_fov": "float", + "waypointMaxWeight": "float", + "waypointWeight": "float", }, "autonav_nav_resolver": { @@ -137,7 +140,22 @@ var addressKeys = { "autonav_image_combiner": { "internal_title": "[Image Combiner]", "map_res": "int" - } + }, + + "autonav_playback": { + "internal_title": "[Playback]", + "record_imu": "bool", + "record_gps": "bool", + "record_position": "bool", + "record_feedback": "bool", + "record_motor_debug": "bool", + "record_raw_cameras": "bool", + "record_filtered_cameras": "bool", + "record_astar": "bool", + "record_autonomous": "bool", + "record_manual": "bool", + "frame_rate": "int" + } } var conbusDevices = { diff --git a/display/scripts/main.js b/display/scripts/main.js index 409c4a15..cc70b577 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -24,6 +24,7 @@ $(document).ready(function () { send({ op: "broadcast" }); send({ op: "get_nodes" }); + send({ op: "get_presets" }); const waitInterval = setInterval(() => { if (deviceStates["autonav_serial_can"] != 3) { @@ -48,19 +49,47 @@ $(document).ready(function () { $(".connecting").hide(); $("#main").show(); }, 1000); + + setTimeout(() => { + if(websocket.readyState == 1) + { + send({ op: "get_presets" }); + } + }, 3000); }; websocket.onmessage = function (event) { const messages = event.data.split("\n"); - for (const message of messages) - { + for (const message of messages) { const obj = JSON.parse(message); const { op, topic } = obj; - + if (op == "data") { onTopicData(topic, obj); } - + + if (op == "get_presets_callback") { + const presets = obj.presets; + const presetElement = $("#dropdown_elements"); + presetElement.empty(); + for (const preset of presets) { + const dropdownItem = $(`
  • ${preset}
    • `); + presetElement.append(dropdownItem); + + dropdownItem.on("click", function () { + const preset_name = $(this).children().attr("data-value"); + send({ + op: "set_active_preset", + preset: preset_name + }); + send({ op: "get_presets" }); + }); + } + + current_preset = obj.active_preset; + $("#active_preset_value").text(current_preset); + } + if (op == "get_nodes_callback") { console.log(obj); for (let i = 0; i < obj.nodes.length; i++) { @@ -74,8 +103,7 @@ $(document).ready(function () { const statemap = obj.states; if (node in statemap) { - if (node == "rosbridge_websocket" || node == "rosapi" || node == "scr_core" || node == "rosapi_params") - { + if (node == "rosbridge_websocket" || node == "rosapi" || node == "scr_core" || node == "rosapi_params") { continue; } @@ -89,8 +117,7 @@ $(document).ready(function () { } } - for (const key in obj.configs) - { + for (const key in obj.configs) { config[key] = obj.configs[key]; } regenerateConfig(); @@ -100,6 +127,10 @@ $(document).ready(function () { $("#var_system_state").text(system["state"] == 0 ? "Diabled" : system["state"] == 1 ? "Autonomous" : system["state"] == 2 ? "Manual" : "Shutdown"); $("#var_system_mode").text(system["mode"] == 0 ? "Competition" : system["mode"] == 1 ? "Simulation" : "Practice"); $("#var_system_mobility").text(system["mobility"] ? "Enabled" : "Disabled"); + + // Update some buttons + $("#checkbox_system_mobility").prop("checked", system["mobility"]); + $("#input_system_state").val(system["state"]); } } }; @@ -122,7 +153,14 @@ $(document).ready(function () { }; } - createWebsocket(); + if (!development_mode) { + createWebsocket(); + } else { + $("#connecting-state").text("Updating Data"); + $(".connecting-input").hide(); + $(".connecting").hide(); + $("#main").show(); + } var sendQueue = []; @@ -320,7 +358,9 @@ $(document).ready(function () { } if (topic == "/scr/configuration") { - console.log(msg); + const { device, json } = msg; + config[device] = JSON.parse(json); + regenerateConfig(); return; } @@ -520,6 +560,7 @@ $(document).ready(function () { $(".dropdown-menu a").on("click", function () { const parentDataTarget = $(this).parents(".dropdown").attr("data-target"); + console.log(parentDataTarget); if (parentDataTarget == "system_state") { const id = $(this).attr("data-value"); systemState.state = parseInt(id); @@ -539,6 +580,31 @@ $(document).ready(function () { } }); + $("#save_preset_mode").on("click", function () { + send({ + op: "save_preset_mode" + }); + send({ op: "get_presets" }); + }); + + $("#save_preset_as").on("click", function () { + const preset_name = $("#preset_save_name").val(); + send({ + op: "save_preset_as", + preset: preset_name + }); + send({ op: "get_presets" }); + $("#preset_save_name").val(""); + }); + + $("#delete_preset").on("click", function () { + send({ + op: "delete_preset", + preset: current_preset + }); + send({ op: "get_presets" }); + }); + $("#checkbox_system_mobility").on("change", function () { systemState.mobility = $(this).is(":checked"); setSystemState(); @@ -569,40 +635,40 @@ $(document).ready(function () { function generateElementForConfiguration(data, type, device, text) { if (type == "bool") { const checked = data == 1; - + // Create a dropdown const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const select = document.createElement("select"); select.classList.add("form-select"); select.onchange = function () { - config[device][text] = select.value; + config[device][text] = select.value == 1 ? true : false; send({ op: "configuration", device: device, json: config[device], }); } - + const optionTrue = document.createElement("option"); optionTrue.value = 1; optionTrue.innerText = "True"; optionTrue.selected = checked; - + const optionFalse = document.createElement("option"); optionFalse.value = 0; optionFalse.innerText = "False"; optionFalse.selected = !checked; - + select.appendChild(optionTrue); select.appendChild(optionFalse); - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(select); return div; @@ -611,7 +677,7 @@ $(document).ready(function () { const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const input = document.createElement("input"); input.type = "number"; input.classList.add("form-control"); @@ -624,11 +690,11 @@ $(document).ready(function () { json: config[device], }); } - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(input); return div; @@ -637,7 +703,7 @@ $(document).ready(function () { const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const input = document.createElement("input"); input.type = "number"; input.classList.add("form-control"); @@ -650,11 +716,11 @@ $(document).ready(function () { json: config[device], }); } - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(input); return div; @@ -694,7 +760,7 @@ $(document).ready(function () { const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(inputX); div.appendChild(inputY); @@ -703,7 +769,7 @@ $(document).ready(function () { else if (type == "parallelogram.int") { const div = document.createElement("div"); div.classList.add("input-group", "mb-3"); - + function createCoordinateInput(value, onChangeHandler) { const input = document.createElement("input"); input.type = "number"; @@ -712,7 +778,7 @@ $(document).ready(function () { input.onchange = onChangeHandler; return input; } - + const inputX1 = createCoordinateInput(data[0][0], function () { config[device][text][0][0] = parseInt(inputX1.value); send({ @@ -721,7 +787,7 @@ $(document).ready(function () { json: config[device], }); }); - + const inputY1 = createCoordinateInput(data[0][1], function () { config[device][text][0][1] = parseInt(inputY1.value); send({ @@ -730,7 +796,7 @@ $(document).ready(function () { json: config[device], }); }); - + const inputX2 = createCoordinateInput(data[1][0], function () { config[device][text][1][0] = parseInt(inputX2.value); send({ @@ -739,7 +805,7 @@ $(document).ready(function () { json: config[device], }); }); - + const inputY2 = createCoordinateInput(data[1][1], function () { config[device][text][1][1] = parseInt(inputY2.value); send({ @@ -748,7 +814,7 @@ $(document).ready(function () { json: config[device], }); }); - + const inputX3 = createCoordinateInput(data[2][0], function () { config[device][text][2][0] = parseInt(inputX3.value); send({ @@ -757,7 +823,7 @@ $(document).ready(function () { json: config[device], }); }); - + const inputY3 = createCoordinateInput(data[2][1], function () { config[device][text][2][1] = parseInt(inputY3.value); send({ @@ -766,7 +832,7 @@ $(document).ready(function () { json: config[device], }); }); - + const inputX4 = createCoordinateInput(data[3][0], function () { config[device][text][3][0] = parseInt(inputX4.value); send({ @@ -775,7 +841,7 @@ $(document).ready(function () { json: config[device], }); }); - + const inputY4 = createCoordinateInput(data[3][1], function () { config[device][text][3][1] = parseInt(inputY4.value); send({ @@ -784,11 +850,11 @@ $(document).ready(function () { json: config[device], }); }); - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(inputX1); div.appendChild(inputY1); @@ -798,19 +864,19 @@ $(document).ready(function () { div.appendChild(inputY3); div.appendChild(inputX4); div.appendChild(inputY4); - return div; + return div; } else { const options = addressKeys[device][text]; - + if (typeof options == "object") { const index = data; - + // Create a dropdown const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const select = document.createElement("select"); select.classList.add("form-select"); select.onchange = function () { @@ -821,7 +887,7 @@ $(document).ready(function () { json: config[device], }); } - + for (let i = 0; i < Object.keys(options).length; i++) { const option = document.createElement("option"); option.value = i; @@ -829,22 +895,22 @@ $(document).ready(function () { option.innerText = options[i]; select.appendChild(option); } - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(select); return div; } } } - + const regenerateConfig = () => { - const configElement = $("#configuration"); + const configElement = $("#options"); configElement.empty(); - + for (const deviceId in config) { const deviceConfig = config[deviceId]; if (addressKeys[deviceId] == undefined) { @@ -853,13 +919,13 @@ $(document).ready(function () { // configElement.append(alert); continue; } - + const title = addressKeys[deviceId]["internal_title"]; const deviceElement = $(`
    `); deviceElement.append(`
    ${title}
    `); const deviceBody = $(`
    `); deviceElement.append(deviceBody); - + for (const address of Object.keys(deviceConfig).sort()) { const data = deviceConfig[address]; const type = addressKeys[deviceId][address]; @@ -868,24 +934,24 @@ $(document).ready(function () { deviceBody.append(alert); continue; } - + const inputElement = generateElementForConfiguration(data, type, deviceId, address); deviceBody.append(inputElement); } - + for (const address in addressKeys[deviceId]) { if (address in deviceConfig || address == "internal_title") { continue; } - + const alert = $(``); deviceBody.append(alert); } - + configElement.append(deviceElement); } } - + function send(obj) { sendQueue.push(obj); } diff --git a/display/styles/index.css b/display/styles/index.css index 6bb7dc23..563b8875 100644 --- a/display/styles/index.css +++ b/display/styles/index.css @@ -141,6 +141,30 @@ span[data-state="5"] { margin: 1rem 0; } +.controls-row { + display: flex; + flex-direction: row; + gap: 1rem; +} + +.controls-row > button { + text-wrap: nowrap; +} + +#controls { + display: flex; + flex-direction: column; + gap: 1rem; + width: fit-content; +} + +#options { + display: flex; + flex-direction: column; + gap: 1rem; + margin: 1rem 0; +} + #preferences { display: flex; flex-direction: column; @@ -153,22 +177,21 @@ span[data-state="5"] { #images { display: flex; - flex-direction: row; + flex-direction: column; gap: 1rem; - flex-wrap: wrap; } -#images > canvas[data-type="regular"] { - width: 640px !important; - height: 480px !important; +img[data-type="regular"] { + width: 480px !important; + height: 640px !important; } -#images > canvas[data-type="bigboy"] { +img[data-type="bigboy"] { width: 800px !important; height: 800px !important; } -#images > canvas[data-type="small"] { +img[data-type="small"] { width: 80px !important; height: 80px !important; } @@ -183,4 +206,27 @@ span[data-state="5"] { text-align: center; border-radius: 5px; border: 1px solid #ced4da; +} + +.row { + display: flex; + flex-direction: row; + gap: 1rem; + flex-wrap: wrap; + width: fit-content; + height: min-content; +} + +#dashboard { + display: flex; + flex-wrap: wrap; + gap: 1rem; +} + +#vtest { + min-width: 1000px; +} + +#active_preset_value { + color: #ff7bff; } \ No newline at end of file From 8179d63accd9da2bb1ec29c1781a0009a3452dc5 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 17:39:50 -0500 Subject: [PATCH 069/113] Try and add left camera --- setup/etc/autonav.rules | 3 +++ 1 file changed, 3 insertions(+) diff --git a/setup/etc/autonav.rules b/setup/etc/autonav.rules index cde1f625..cb112c6e 100644 --- a/setup/etc/autonav.rules +++ b/setup/etc/autonav.rules @@ -12,3 +12,6 @@ SUBSYSTEM=="usb", ATTRS{idVendor}=="28de", MODE="0666" # Safety Lights PICO KERNEL=="ttyACM*", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="000a", ATTRS{serial}=="E660D051138A8531", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-mc-safetylights" + +# Cameras +KERNEL=="ttyUSB*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="08e5", ATTRS{serial}=="3F47331F", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-camera-left" \ No newline at end of file From c9f5ea3d1cb6e73a5073f815c13babbe1bc614d3 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 17:47:04 -0500 Subject: [PATCH 070/113] Update autonav.rules to add right camera support --- setup/etc/autonav.rules | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/setup/etc/autonav.rules b/setup/etc/autonav.rules index cb112c6e..5d4c7c73 100644 --- a/setup/etc/autonav.rules +++ b/setup/etc/autonav.rules @@ -14,4 +14,4 @@ SUBSYSTEM=="usb", ATTRS{idVendor}=="28de", MODE="0666" KERNEL=="ttyACM*", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="000a", ATTRS{serial}=="E660D051138A8531", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-mc-safetylights" # Cameras -KERNEL=="ttyUSB*", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="08e5", ATTRS{serial}=="3F47331F", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-camera-left" \ No newline at end of file +SUBSYSTEM=="video4linux", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="08e5", ATTR{index}=="0", MODE="0664", GROUP="video", SYMLINK+="autonav-camera-right" \ No newline at end of file From 21908731795a6a7b61b456edd78ba547d34e272d Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 17:52:09 -0500 Subject: [PATCH 071/113] big code --- autonav_ws/src/autonav_serial/src/camera.py | 6 +++--- setup/etc/autonav.rules | 5 +---- 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index de781ab2..6bc0520d 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -112,9 +112,9 @@ def camera_worker(self): def main(): rclpy.init() - node_left = CameraNode("left", "/dev/autonav_camera_left") - node_right = CameraNode("right", "/dev/autonav_camera_right") - Node.run_nodes([node_left, node_right]) + # node_left = CameraNode("left", "/dev/autonav_camera_left") + node_right = CameraNode("right", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_3F47331F-video-index0") + Node.run_nodes([node_right]) rclpy.shutdown() diff --git a/setup/etc/autonav.rules b/setup/etc/autonav.rules index 5d4c7c73..f6cd8301 100644 --- a/setup/etc/autonav.rules +++ b/setup/etc/autonav.rules @@ -11,7 +11,4 @@ KERNEL=="hidraw*", KERNELS=="*28DE:*", MODE="0666", TAG+="uaccess" SUBSYSTEM=="usb", ATTRS{idVendor}=="28de", MODE="0666" # Safety Lights PICO -KERNEL=="ttyACM*", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="000a", ATTRS{serial}=="E660D051138A8531", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-mc-safetylights" - -# Cameras -SUBSYSTEM=="video4linux", ATTRS{idVendor}=="046d", ATTRS{idProduct}=="08e5", ATTR{index}=="0", MODE="0664", GROUP="video", SYMLINK+="autonav-camera-right" \ No newline at end of file +KERNEL=="ttyACM*", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="000a", ATTRS{serial}=="E660D051138A8531", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-mc-safetylights" \ No newline at end of file From 39f4ea03c85baff19eed7a2a1638af87510db343 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 17:54:00 -0500 Subject: [PATCH 072/113] Update camera.py to use correct camera index for left and right cameras --- autonav_ws/src/autonav_serial/src/camera.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 6bc0520d..cacfd10c 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -112,9 +112,9 @@ def camera_worker(self): def main(): rclpy.init() - # node_left = CameraNode("left", "/dev/autonav_camera_left") + node_left = CameraNode("left", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_8174526F-video-index0") node_right = CameraNode("right", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_3F47331F-video-index0") - Node.run_nodes([node_right]) + Node.run_nodes([node_left, node_right]) rclpy.shutdown() From 7c00a6424123d404036af00c58d73255826ed5e5 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 18:20:07 -0500 Subject: [PATCH 073/113] Add vectornav/imu to launch files --- autonav_ws/src/autonav_launch/launch/competition.xml | 1 + autonav_ws/src/autonav_launch/launch/managed_steam.xml | 1 + autonav_ws/src/autonav_launch/launch/practice.xml | 1 + 3 files changed, 3 insertions(+) diff --git a/autonav_ws/src/autonav_launch/launch/competition.xml b/autonav_ws/src/autonav_launch/launch/competition.xml index b10de6e2..263a5cd9 100644 --- a/autonav_ws/src/autonav_launch/launch/competition.xml +++ b/autonav_ws/src/autonav_launch/launch/competition.xml @@ -21,6 +21,7 @@ + diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index c4fbc83e..31b2b12a 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -21,6 +21,7 @@ + diff --git a/autonav_ws/src/autonav_launch/launch/practice.xml b/autonav_ws/src/autonav_launch/launch/practice.xml index e22bea06..3fd07e86 100644 --- a/autonav_ws/src/autonav_launch/launch/practice.xml +++ b/autonav_ws/src/autonav_launch/launch/practice.xml @@ -20,6 +20,7 @@ + From b34988a8a69d89780264e90961ad00d123de5638 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 18:37:55 -0500 Subject: [PATCH 074/113] Fix vectornav launch name --- autonav_ws/src/autonav_launch/launch/competition.xml | 2 +- autonav_ws/src/autonav_launch/launch/managed_steam.xml | 2 +- autonav_ws/src/autonav_launch/launch/practice.xml | 2 +- autonav_ws/src/autonav_serial/src/safety_lights.py | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/competition.xml b/autonav_ws/src/autonav_launch/launch/competition.xml index 263a5cd9..33e17eb1 100644 --- a/autonav_ws/src/autonav_launch/launch/competition.xml +++ b/autonav_ws/src/autonav_launch/launch/competition.xml @@ -21,7 +21,7 @@ - + diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 31b2b12a..cec156a8 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -21,7 +21,7 @@ - + diff --git a/autonav_ws/src/autonav_launch/launch/practice.xml b/autonav_ws/src/autonav_launch/launch/practice.xml index 3fd07e86..8eee093c 100644 --- a/autonav_ws/src/autonav_launch/launch/practice.xml +++ b/autonav_ws/src/autonav_launch/launch/practice.xml @@ -20,7 +20,7 @@ - + diff --git a/autonav_ws/src/autonav_serial/src/safety_lights.py b/autonav_ws/src/autonav_serial/src/safety_lights.py index 30bf6b97..a7ec2717 100644 --- a/autonav_ws/src/autonav_serial/src/safety_lights.py +++ b/autonav_ws/src/autonav_serial/src/safety_lights.py @@ -71,7 +71,7 @@ def onSafetyLightsReceived(self, lights: SafetyLights): data["red"] = lights.red data["green"] = lights.green data["blue"] = lights.blue - data["blink_period"] = 500 + data["blink_period"] = 500 / 10 self.writeQueueLock.acquire() self.writeQueue.append(data) From 2fb1e293a0c3437169f76ffdc05a99d17c568cbc Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 18:58:42 -0500 Subject: [PATCH 075/113] Sigkill python processes --- autonav_ws/src/scr/scr/node.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index 8eb0cd23..fd97f20f 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -127,7 +127,8 @@ def on_system_state(self, msg: SystemState): # If the system state is shutdown, kill this node killing the proces if msg.state == SystemStateEnum.SHUTDOWN: - os.kill(os.getpid(), signal.SIGINT) + self.get_logger().info("Received shutdown signal, shutting down") + os.kill(os.getpid(), signal.SIGKILL) return oldState = SystemState() From 53256e8325cb7a55c3873ba7571d18f2b3653ed4 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 19:01:44 -0500 Subject: [PATCH 076/113] Change cpp to use sigkill --- autonav_ws/src/scr/src/node.cpp | 2 +- display/index.html | 6 +++--- display/styles/index.css | 3 +-- 3 files changed, 5 insertions(+), 6 deletions(-) diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index a643cfaa..ec7be734 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -59,7 +59,7 @@ namespace SCR // If the new system state is shutdown, just exit the process if (msg.state == static_cast(SCR::SystemState::SHUTDOWN)) { - kill(getpid(), SIGINT); + kill(getpid(), SIGKILL); } SCR::SystemState newStateEnum = static_cast(msg.state); diff --git a/display/index.html b/display/index.html index 07099d77..af9f77d2 100644 --- a/display/index.html +++ b/display/index.html @@ -109,15 +109,15 @@
    Waypoints:

    Vision

    -
    +
    -
    +
    -
    +
    diff --git a/display/styles/index.css b/display/styles/index.css index 563b8875..096c1464 100644 --- a/display/styles/index.css +++ b/display/styles/index.css @@ -208,10 +208,9 @@ img[data-type="small"] { border: 1px solid #ced4da; } -.row { +.roww { display: flex; flex-direction: row; - gap: 1rem; flex-wrap: wrap; width: fit-content; height: min-content; From 31a4a347cef6a02c3e8a6ef2b89b4af3a249c354 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 19:04:49 -0500 Subject: [PATCH 077/113] Make core node end its process --- autonav_ws/src/scr_controller/src/core.cpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/autonav_ws/src/scr_controller/src/core.cpp b/autonav_ws/src/scr_controller/src/core.cpp index 913229bc..0d4fe74a 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include "scr/json.hpp" using json = nlohmann::json; @@ -110,6 +111,12 @@ class CoreNode : public rclcpp::Node mode = (SCR::SystemMode)request->mode; mobility = request->mobility; + if (state == SCR::SystemState::SHUTDOWN) + { + // Start a 5 second timer to kill the node + mode_timer = this->create_wall_timer(std::chrono::seconds(5), std::bind(&CoreNode::kill_self, this)); + } + // Send the response response->success = true; @@ -396,6 +403,11 @@ class CoreNode : public rclcpp::Node response->ok = true; } + void kill_self() + { + kill(getpid(), SIGKILL); + } + private: struct publishers publishers; struct services services; From 2a2cf5ab36859a4c5d92403bd245ef3408dd668d Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:03:27 -0500 Subject: [PATCH 078/113] noahgram --- .../src/autonav_vision/src/transformations.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 9a113cd8..6989ba27 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -111,6 +111,18 @@ def order_points(self, pts): rect[3] = pts[np.argmax(diff)] # return the ordered coordinates return rect + + def epic_noah_transform(self, image, top_width, bottom_width, height, offset): + dst = np.array([ + [0, 0], + [top_width+offset-1, 0], + [bottom_width+offset-1, height - 1], + [0, height - 1]], dtype="float32") + # compute the perspective transform matrix and then apply it + M = cv2.getPerspectiveTransform(rect, dst) + warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) + # return the warped image + return warped def four_point_transform(self, image, pts): @@ -213,7 +225,8 @@ def apply_perspective_transform(self, img): return img pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] - mask = self.four_point_transform(img, np.array(pts)) + # mask = self.four_point_transform(img, np.array(pts)) + mask = self.epic_noah_transform(img, 320, 240, 640, -20 if self.dir == "left" else 20) return mask def onImageReceived(self, image: CompressedImage): From cc86dc3595897436bdc133d3645283eef2c104f6 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:07:28 -0500 Subject: [PATCH 079/113] refactor: Update epic_noah_transform method to accept additional points parameter --- .../src/autonav_vision/src/transformations.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 6989ba27..ac9ba01b 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -112,7 +112,23 @@ def order_points(self, pts): # return the ordered coordinates return rect - def epic_noah_transform(self, image, top_width, bottom_width, height, offset): + def epic_noah_transform(self, image, pts, top_width, bottom_width, height, offset): + + rect = self.order_points(pts) + (tl, tr, br, bl) = rect + # compute the width of the new image, which will be the + # maximum distance between bottom-right and bottom-left + # x-coordiates or the top-right and top-left x-coordinates + widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2)) + widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2)) + maxWidth = max(int(widthA), int(widthB)) + # compute the height of the new image, which will be the + # maximum distance between the top-right and bottom-right + # y-coordinates or the top-left and bottom-left y-coordinates + heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2)) + heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2)) + maxHeight = max(int(heightA), int(heightB)) + dst = np.array([ [0, 0], [top_width+offset-1, 0], From fa957a5bd7dcbed4014e41ebb0e4e7c460be9444 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:08:16 -0500 Subject: [PATCH 080/113] add np.array --- autonav_ws/src/autonav_vision/src/transformations.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index ac9ba01b..16ca91e9 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -242,7 +242,7 @@ def apply_perspective_transform(self, img): pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] # mask = self.four_point_transform(img, np.array(pts)) - mask = self.epic_noah_transform(img, 320, 240, 640, -20 if self.dir == "left" else 20) + mask = self.epic_noah_transform(img, np.array(pts), 320, 240, 640, -20 if self.dir == "left" else 20) return mask def onImageReceived(self, image: CompressedImage): From 335c1609e14bc5ffc986fd83ed9fd561ce33bd71 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:10:09 -0500 Subject: [PATCH 081/113] refactor: Update epic_noah_transform method to accept additional points parameter --- autonav_ws/src/autonav_vision/src/transformations.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 16ca91e9..2d05ed09 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -57,6 +57,10 @@ def __init__(self): self.parallelogram_left = [[500, 405], [260, 400], [210, 640], [640, 640]] self.parallelogram_right = [[0, 415], [195, 390], [260, 640], [0, 640]] + self.top_width = 320 + self.bottom_width = 240 + self.offset = 20 + # Disabling self.disable_blur = False self.disable_hsv = False @@ -242,7 +246,7 @@ def apply_perspective_transform(self, img): pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] # mask = self.four_point_transform(img, np.array(pts)) - mask = self.epic_noah_transform(img, np.array(pts), 320, 240, 640, -20 if self.dir == "left" else 20) + mask = self.epic_noah_transform(img, np.array(pts), self.config.top_width, self.config.bottom_width, 640, -1 * self.config.offset if self.dir == "left" else self.config.offset) return mask def onImageReceived(self, image: CompressedImage): From 63df58932d6f7b196ad6ea1da3ed51c619d2fdc7 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:21:51 -0500 Subject: [PATCH 082/113] refactor: Update epic_noah_transform method to accept additional points parameter --- .../src/autonav_vision/src/transformations.py | 19 +++++++++++++------ display/scripts/globals.js | 3 +++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 2d05ed09..6ed4e1df 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -133,11 +133,18 @@ def epic_noah_transform(self, image, pts, top_width, bottom_width, height, offse heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2)) maxHeight = max(int(heightA), int(heightB)) - dst = np.array([ - [0, 0], - [top_width+offset-1, 0], - [bottom_width+offset-1, height - 1], - [0, height - 1]], dtype="float32") + if self.dir == "left": + dst = np.array([ + [240 - bottom_width - offset, 0], + [240, 0], + [240, height - 1], + [240 - bottom_width - offset, height - 1]], dtype="float32") + else: + dst = np.array([ + [240 + bottom_width + offset, 0], + [240, 0], + [240, height - 1], + [240 + bottom_width + offset, height - 1]], dtype="float32") # compute the perspective transform matrix and then apply it M = cv2.getPerspectiveTransform(rect, dst) warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) @@ -246,7 +253,7 @@ def apply_perspective_transform(self, img): pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] # mask = self.four_point_transform(img, np.array(pts)) - mask = self.epic_noah_transform(img, np.array(pts), self.config.top_width, self.config.bottom_width, 640, -1 * self.config.offset if self.dir == "left" else self.config.offset) + mask = self.epic_noah_transform(img, np.array(pts), self.config.top_width, self.config.bottom_width, 640, self.config.offset) return mask def onImageReceived(self, image: CompressedImage): diff --git a/display/scripts/globals.js b/display/scripts/globals.js index e3fddb4a..2e3d45d7 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -72,6 +72,9 @@ var addressKeys = { "disable_region_of_disinterest": "bool", "parallelogram_left": "parallelogram.int", "parallelogram_right": "parallelogram.int", + "top_width": "float", + "bottom_width": "float", + "offset": "float" }, "autonav_vision_expandifier": { From 799bbf336251be3ca2d8c27cab3f788f2b9f3ee3 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:24:45 -0500 Subject: [PATCH 083/113] bottom_width = top_width --- autonav_ws/src/autonav_vision/src/transformations.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 6ed4e1df..a25b093c 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -138,13 +138,13 @@ def epic_noah_transform(self, image, pts, top_width, bottom_width, height, offse [240 - bottom_width - offset, 0], [240, 0], [240, height - 1], - [240 - bottom_width - offset, height - 1]], dtype="float32") + [240 - top_width - offset, height - 1]], dtype="float32") else: dst = np.array([ [240 + bottom_width + offset, 0], [240, 0], [240, height - 1], - [240 + bottom_width + offset, height - 1]], dtype="float32") + [240 + top_width + offset, height - 1]], dtype="float32") # compute the perspective transform matrix and then apply it M = cv2.getPerspectiveTransform(rect, dst) warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) From 8ea35adc8ab4f6d5da9f2e191053a83aa717acc4 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:27:10 -0500 Subject: [PATCH 084/113] "were in" - noah --- autonav_ws/src/autonav_vision/src/transformations.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index a25b093c..c0b11c47 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -141,10 +141,10 @@ def epic_noah_transform(self, image, pts, top_width, bottom_width, height, offse [240 - top_width - offset, height - 1]], dtype="float32") else: dst = np.array([ - [240 + bottom_width + offset, 0], [240, 0], - [240, height - 1], - [240 + top_width + offset, height - 1]], dtype="float32") + [240, 240 + bottom_width + offset], + [240 + top_width + offset, height - 1], + [240, height - 1]], dtype="float32") # compute the perspective transform matrix and then apply it M = cv2.getPerspectiveTransform(rect, dst) warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) From 4015b32269b50d14bc454905cae4e48bf9a14c53 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:29:19 -0500 Subject: [PATCH 085/113] refactor: Combine occupancy grids and publish combined grid as image --- .../src/autonav_vision/src/combination.py | 30 ++++++++++++++++++- 1 file changed, 29 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 47b01d54..ff914b1d 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -64,10 +64,38 @@ def scale_grid(self, grid): scaled_grid.data[y * 40 + x] = grid.data[y * 80 + (2 * x)] return scaled_grid - + def try_combine_grids(self): if self.grid_left is None or self.grid_right is None: return + + combined_grid = OccupancyGrid() + combined_grid.header = self.grid_left.header + combined_grid.info = self.grid_left.info + combined_grid.data = [0] * len(self.grid_left.data) + for i in range(len(self.grid_left.data)): + if self.grid_left.data[i] > 0 or self.grid_right.data[i] > 0: + # Need to figure out what the value actually is: 255, 100, 1? + combined_grid.data[i] = self.grid_left.data[i] if self.grid_left.data[i] > self.grid_right.data[i] else self.grid_right.data[i] + else: + combined_grid.data[i] = 0 + + self.grid_left = None + self.grid_right = None + self.combined_grid_publisher.publish(combined_grid) + + # Publish the combined grid as an image + preview_image = np.zeros((80, 80), dtype=np.uint8) + for i in range(80): + for j in range(80): + preview_image[i, j] = combined_grid.data[i * 80 + j] * 255 / 100 - 255 + preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB) + compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) + self.combined_grid_image_publisher.publish(compressed_image) + + def try_combine_grids_old(self): + if self.grid_left is None or self.grid_right is None: + return # Scale down grids scaled_left = self.scale_grid(self.grid_left) From 0744d1998592095b860e5f5d708e9e009b03ecf0 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:30:45 -0500 Subject: [PATCH 086/113] "its good" - noah --- autonav_ws/src/autonav_vision/src/transformations.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index c0b11c47..04eb7557 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -142,7 +142,7 @@ def epic_noah_transform(self, image, pts, top_width, bottom_width, height, offse else: dst = np.array([ [240, 0], - [240, 240 + bottom_width + offset], + [240 + bottom_width + offset, 0], [240 + top_width + offset, height - 1], [240, height - 1]], dtype="float32") # compute the perspective transform matrix and then apply it From b4e483c9d96b01bc8911bdf27df129109c1dfb8c Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:39:47 -0500 Subject: [PATCH 087/113] Fix preview image --- autonav_ws/src/autonav_vision/src/combination.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index ff914b1d..4fcafad8 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -88,7 +88,7 @@ def try_combine_grids(self): preview_image = np.zeros((80, 80), dtype=np.uint8) for i in range(80): for j in range(80): - preview_image[i, j] = combined_grid.data[i * 80 + j] * 255 / 100 - 255 + preview_image[i, j] = 0 if combined_grid.data[i * 80 + j] <= 10 else 255 preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB) compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) self.combined_grid_image_publisher.publish(compressed_image) From 8b7910b062a16f4bf6ef54c22fe92f81d83b344c Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:42:08 -0500 Subject: [PATCH 088/113] refactor: Rename try_combine_grids to try_combine_grids_new for clarity --- autonav_ws/src/autonav_vision/src/combination.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 4fcafad8..549698cb 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -65,7 +65,7 @@ def scale_grid(self, grid): return scaled_grid - def try_combine_grids(self): + def try_combine_grids_new(self): if self.grid_left is None or self.grid_right is None: return @@ -93,7 +93,7 @@ def try_combine_grids(self): compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) self.combined_grid_image_publisher.publish(compressed_image) - def try_combine_grids_old(self): + def try_combine_grids(self): if self.grid_left is None or self.grid_right is None: return From edabf78cf9c317631440b8b37b1c0210b594e77f Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:45:10 -0500 Subject: [PATCH 089/113] refactor: Rename try_combine_grids to try_combine_grids_new for clarity --- autonav_ws/src/autonav_vision/src/combination.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 549698cb..4fcafad8 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -65,7 +65,7 @@ def scale_grid(self, grid): return scaled_grid - def try_combine_grids_new(self): + def try_combine_grids(self): if self.grid_left is None or self.grid_right is None: return @@ -93,7 +93,7 @@ def try_combine_grids_new(self): compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) self.combined_grid_image_publisher.publish(compressed_image) - def try_combine_grids(self): + def try_combine_grids_old(self): if self.grid_left is None or self.grid_right is None: return From 84d896e7735d28f930d619107580a1cf5ff155d9 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Mon, 20 May 2024 20:58:06 -0500 Subject: [PATCH 090/113] update parallel to stay constant width --- autonav_ws/src/autonav_vision/src/transformations.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 04eb7557..04c3d250 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -136,15 +136,15 @@ def epic_noah_transform(self, image, pts, top_width, bottom_width, height, offse if self.dir == "left": dst = np.array([ [240 - bottom_width - offset, 0], - [240, 0], - [240, height - 1], + [240 - offset, 0], + [240 - offset, height - 1], [240 - top_width - offset, height - 1]], dtype="float32") else: dst = np.array([ - [240, 0], + [240 + offset, 0], [240 + bottom_width + offset, 0], [240 + top_width + offset, height - 1], - [240, height - 1]], dtype="float32") + [240 + offset, height - 1]], dtype="float32") # compute the perspective transform matrix and then apply it M = cv2.getPerspectiveTransform(rect, dst) warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) From 3db31bf0b700e2be163fc5b5effccbf5a92a6b73 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 09:26:22 -0500 Subject: [PATCH 091/113] Add debug logging helper functions --- autonav_ws/src/scr/include/scr/node.hpp | 4 ++++ autonav_ws/src/scr/scr/node.py | 3 +++ autonav_ws/src/scr/src/node.cpp | 4 ++++ 3 files changed, 11 insertions(+) diff --git a/autonav_ws/src/scr/include/scr/node.hpp b/autonav_ws/src/scr/include/scr/node.hpp index 3d3cb4ed..c85bb9ea 100644 --- a/autonav_ws/src/scr/include/scr/node.hpp +++ b/autonav_ws/src/scr/include/scr/node.hpp @@ -105,6 +105,10 @@ namespace SCR /// @param data The message to log void log(std::string data); + /// @brief Logs a message to the console + /// @param message The message to log + void log_debug(std::string message); + private: void system_state_callback(const scr_msgs::msg::SystemState msg); void device_state_callback(const scr_msgs::msg::DeviceState msg); diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index fd97f20f..c12c870c 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -82,6 +82,9 @@ def log(self, data): log_packet.node = self.identifier self.logging_publisher.publish(log_packet) + def log_debug(self, message): + self.get_logger().log(message) + def on_device_state(self, msg: DeviceState): """ Called when a device state message is received. diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index ec7be734..73304b56 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -279,4 +279,8 @@ namespace SCR rclcpp::shutdown(); } + void Node::log_debug(std::string message) + { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } } \ No newline at end of file From 0afb5f09603eaae76a16cbbaec8465b55114a9f2 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 09:40:34 -0500 Subject: [PATCH 092/113] Config system now updates with missing keys --- autonav_ws/src/scr_controller/src/core.cpp | 87 ++++++++++++++-------- 1 file changed, 55 insertions(+), 32 deletions(-) diff --git a/autonav_ws/src/scr_controller/src/core.cpp b/autonav_ws/src/scr_controller/src/core.cpp index 0d4fe74a..b1138428 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -217,10 +217,7 @@ class CoreNode : public rclcpp::Node response->success = true; // Publish the new config - scr_msgs::msg::ConfigUpdated config_updated_message; - config_updated_message.device = request->device; - config_updated_message.json = request->json; - publishers.config_updated->publish(config_updated_message); + publish_config(config); } void on_set_active_preset_called(const std::shared_ptr request, std::shared_ptr response) @@ -248,10 +245,7 @@ class CoreNode : public rclcpp::Node configs[device] = cfg; // Publish the new config - scr_msgs::msg::ConfigUpdated config_updated_message; - config_updated_message.device = device; - config_updated_message.json = cfg.dump(); - publishers.config_updated->publish(config_updated_message); + publish_config(cfg); } response->ok = true; @@ -267,12 +261,7 @@ class CoreNode : public rclcpp::Node std::string home = std::getenv("HOME"); std::string path = home + "/.config/autonav/" + name + ".preset"; - // Write the preset to disk, if it exists then overwrite it - std::string jsonStr = nlohmann::json(active_preset).dump(); - std::ofstream file(path); - file << jsonStr; - file.close(); - + write_preset(name, active_preset); response->ok = true; } @@ -310,7 +299,33 @@ class CoreNode : public rclcpp::Node std::string preset = get_preset_for_mode(); if (!preset.empty()) { - active_preset = json::parse(preset); + auto upcoming_preset = json::parse(preset); + + // Checking for missing devices and keys + for (auto const &[device, cfg] : configs) + { + // Check for missing devices + if (upcoming_preset.find(device) == upcoming_preset.end()) + { + // The upcoming preset is missing a config, so we need to add it + upcoming_preset[device] = cfg; + RCLCPP_WARN(this->get_logger(), "Warning: upcoming preset is missing config for device %s", device.c_str()); + continue; + } + + // Check for missing keys + for (auto const &[key, value] : cfg.items()) + { + if (upcoming_preset[device].find(key) == upcoming_preset[device].end()) + { + // The upcoming preset is missing a key, so we need to add it + upcoming_preset[device][key] = value; + RCLCPP_WARN(this->get_logger(), "Warning: upcoming preset is missing key %s for device %s", key.c_str(), device.c_str()); + } + } + } + + active_preset = upcoming_preset; active_preset_name = SCR::systemModeToString(mode); for (auto const &[device, cfg] : active_preset) { @@ -323,26 +338,17 @@ class CoreNode : public rclcpp::Node configs[device] = cfg; // Publish the new config - scr_msgs::msg::ConfigUpdated config_updated_message; - config_updated_message.device = device; - config_updated_message.json = cfg.dump(); - publishers.config_updated->publish(config_updated_message); + publish_config(cfg); } + + write_preset(SCR::systemModeToString(mode), active_preset); return; } // If there is no preset for the current mode, then we need to create one active_preset = configs; active_preset_name = SCR::systemModeToString(mode); - std::string modeStr = SCR::systemModeToString(mode); - std::string home = std::getenv("HOME"); - std::string path = home + "/.config/autonav/" + modeStr + ".preset"; - - // Write the preset to disk, if it exists then overwrite it - std::string jsonStr = nlohmann::json(active_preset).dump(); - std::ofstream file(path); - file << jsonStr; - file.close(); + write_preset(active_preset_name, active_preset); RCLCPP_INFO(this->get_logger(), "Created preset for mode %d", (int)mode); } @@ -393,16 +399,33 @@ class CoreNode : public rclcpp::Node configs[device] = cfg; // Publish the new config - scr_msgs::msg::ConfigUpdated config_updated_message; - config_updated_message.device = device; - config_updated_message.json = cfg.dump(); - publishers.config_updated->publish(config_updated_message); + publish_config(cfg); } } response->ok = true; } + void write_preset(std::string name, json preset) + { + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + name + ".preset"; + + // Write the preset to disk, if it exists then overwrite it + std::string jsonStr = nlohmann::json(preset).dump(); + std::ofstream file(path); + file << jsonStr; + file.close(); + } + + void publish_config(nlohmann::json cfg) + { + scr_msgs::msg::ConfigUpdated config_updated_message; + config_updated_message.device = "preset"; + config_updated_message.json = cfg.dump(); + publishers.config_updated->publish(config_updated_message); + } + void kill_self() { kill(getpid(), SIGKILL); From acd5af3abf8f2c6c4a8fc6a2d51d2b7f06c63c2a Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 09:40:48 -0500 Subject: [PATCH 093/113] Light logging cleanup --- autonav_ws/src/autonav_playback/src/playback.py | 2 -- autonav_ws/src/autonav_vision/src/expandify.cpp | 1 - 2 files changed, 3 deletions(-) diff --git a/autonav_ws/src/autonav_playback/src/playback.py b/autonav_ws/src/autonav_playback/src/playback.py index e128a2fb..52057c45 100644 --- a/autonav_ws/src/autonav_playback/src/playback.py +++ b/autonav_ws/src/autonav_playback/src/playback.py @@ -65,7 +65,6 @@ def init(self): def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) - self.get_logger().info(f"Updated config: {self.jdump(jsonObject)}") def get_default_config(self): return ImageTransformerConfig() @@ -151,7 +150,6 @@ def write_image(self, img: CompressedImage, relative_path: str, idx: int): cv2.imwrite(os.path.join(IMAGE_PATH, f"{idx}.jpg"), cv2Image) def system_state_transition(self, old: SystemState, updated: SystemState): - self.get_logger().info(f"System state transitioned from {old.state} to {updated.state}") if old.state == SystemStateEnum.AUTONOMOUS and updated.state != SystemStateEnum.AUTONOMOUS: self.close_entry() diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 03e3fce1..fec74c39 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -84,7 +84,6 @@ class ExpandifyNode : public SCR::Node void config_updated(json newConfig) override { config = newConfig.template get(); - RCLCPP_INFO(this->get_logger(), "Config updated: vertical_fov: %f, horizontal_fov: %f, map_res: %f", config.vertical_fov, config.horizontal_fov, config.map_res); build_circles(); } From 1ee27044f38dc19aa1ed7e2839baf69ed169faad Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 10:52:46 -0500 Subject: [PATCH 094/113] Stop simulation launch file from autoparameter --- autonav_ws/src/autonav_launch/launch/simulation.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/simulation.xml b/autonav_ws/src/autonav_launch/launch/simulation.xml index cdfcb914..2252c411 100644 --- a/autonav_ws/src/autonav_launch/launch/simulation.xml +++ b/autonav_ws/src/autonav_launch/launch/simulation.xml @@ -3,8 +3,8 @@ - - + + From 14f5f51a1dd37520c930f9cffe20d1a1ef859c80 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 10:52:56 -0500 Subject: [PATCH 095/113] Add ability to turn off autowaypoint calculation --- autonav_ws/src/autonav_nav/src/astar.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 03e703cf..63042404 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -61,6 +61,7 @@ class AStarNodeConfig: def __init__(self): self.waypointPopDistance = 1.1 self.waypointDirection = 0 + self.calculateWaypointDirection = False self.useOnlyWaypoints = False self.waypointDelay = 18 self.waypointWeight = 1.0 @@ -112,6 +113,9 @@ def onReset(self): self.waypoint_start_time = self.config.waypointDelay + time.time() def get_waypoints_for_dir(self): + if not self.config.calculateWaypointDirection: + return simulation_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[self.config.waypointDirection] + # Get out current heading and estimate within 180 degrees which direction we are facing (north or south, 0 and 1 respectively) heading = self.position.theta direction_index = 0 @@ -160,7 +164,7 @@ def create_path(self): cv2.circle(cvimg, (pp[0], pp[1]), 1, (0, 255, 0), 1) cv2.circle(cvimg, (self.best_pos[0], self.best_pos[1]), 1, (255, 0, 0), 1) - cvimg = cv2.resize(cvimg, (800, 800), interpolation=cv2.INTER_NEAREST) + cvimg = cv2.resize(cvimg, (320, 320), interpolation=cv2.INTER_NEAREST) self.pathDebugImagePublisher.publish(CV_BRIDGE.cv2_to_compressed_imgmsg(cvimg)) def reconstruct_path(self, path, current): From 75868746068802a228326a92e8ab4db64ae5eac0 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 10:53:22 -0500 Subject: [PATCH 096/113] Fix playback recording different frame rate for each view --- autonav_ws/src/autonav_playback/src/playback.py | 17 ++++++----------- 1 file changed, 6 insertions(+), 11 deletions(-) diff --git a/autonav_ws/src/autonav_playback/src/playback.py b/autonav_ws/src/autonav_playback/src/playback.py index 52057c45..7ab8dd21 100644 --- a/autonav_ws/src/autonav_playback/src/playback.py +++ b/autonav_ws/src/autonav_playback/src/playback.py @@ -197,19 +197,15 @@ def filtered_callback_left(self, msg: CompressedImage): return self.image_filtered_left = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") - if self.image_filtered_right is not None: - self.filtered_callback_combined() def filtered_callback_right(self, msg: CompressedImage): if not self.config.record_filtered_cameras: return self.image_filtered_right = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") - if self.image_filtered_left is not None: - self.filtered_callback_combined() def filtered_callback_combined(self): - if not self.config.record_filtered_cameras: + if not self.config.record_filtered_cameras or self.image_filtered_left is None or self.image_filtered_right is None: return # Scale the images to the 800 x 800 @@ -225,7 +221,7 @@ def filtered_callback_combined(self): self.image_filtered_right = None def astar_callback(self, msg: CompressedImage): - if not self.config.record_astar: + if not self.config.record_astar or self.image_filtered_left is None or self.image_filtered_right is None or self.image_raw_left is None or self.image_raw_right is None: return # Scale the images to the 800 x 800 @@ -237,24 +233,23 @@ def astar_callback(self, msg: CompressedImage): self.write_image(msg, "astar", self.astar_index) self.astar_index += 1 + self.camera_callback_combined() + self.filtered_callback_combined() + def camera_callback_left(self, msg: CompressedImage): if not self.config.record_raw_cameras: return self.image_raw_left = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") - if self.image_raw_right is not None: - self.camera_callback_combined() def camera_callback_right(self, msg: CompressedImage): if not self.config.record_raw_cameras: return self.image_raw_right = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") - if self.image_raw_left is not None: - self.camera_callback_combined() def camera_callback_combined(self): - if not self.config.record_raw_cameras: + if not self.config.record_raw_cameras or self.image_raw_left is None or self.image_raw_right is None: return img = cv2.hconcat([self.image_raw_left, self.image_raw_right]) From 90435404b357844c0dfc8e06a26ce1ed3c17101b Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 10:53:40 -0500 Subject: [PATCH 097/113] Upscale small images --- autonav_ws/src/autonav_vision/src/combination.py | 1 + 1 file changed, 1 insertion(+) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 4fcafad8..22f7db8b 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -90,6 +90,7 @@ def try_combine_grids(self): for j in range(80): preview_image[i, j] = 0 if combined_grid.data[i * 80 + j] <= 10 else 255 preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB) + preview_image = cv2.resize(preview_image, (320, 320)) compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) self.combined_grid_image_publisher.publish(compressed_image) From df6b71c0755162725f627fb5b1c0d0c7b191a1f8 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 10:53:50 -0500 Subject: [PATCH 098/113] Fix a silly config issue --- autonav_ws/src/scr_controller/src/core.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/autonav_ws/src/scr_controller/src/core.cpp b/autonav_ws/src/scr_controller/src/core.cpp index b1138428..260e9e9d 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -213,11 +213,11 @@ class CoreNode : public rclcpp::Node // Store the config configs[request->device] = config; + // Publish the new config + publish_config(request->device, config); + // Send the response response->success = true; - - // Publish the new config - publish_config(config); } void on_set_active_preset_called(const std::shared_ptr request, std::shared_ptr response) @@ -245,7 +245,7 @@ class CoreNode : public rclcpp::Node configs[device] = cfg; // Publish the new config - publish_config(cfg); + publish_config(device, cfg); } response->ok = true; @@ -338,7 +338,7 @@ class CoreNode : public rclcpp::Node configs[device] = cfg; // Publish the new config - publish_config(cfg); + publish_config(device, cfg); } write_preset(SCR::systemModeToString(mode), active_preset); @@ -349,7 +349,6 @@ class CoreNode : public rclcpp::Node active_preset = configs; active_preset_name = SCR::systemModeToString(mode); write_preset(active_preset_name, active_preset); - RCLCPP_INFO(this->get_logger(), "Created preset for mode %d", (int)mode); } @@ -399,7 +398,7 @@ class CoreNode : public rclcpp::Node configs[device] = cfg; // Publish the new config - publish_config(cfg); + publish_config(device, cfg); } } @@ -418,10 +417,10 @@ class CoreNode : public rclcpp::Node file.close(); } - void publish_config(nlohmann::json cfg) + void publish_config(std::string device, nlohmann::json cfg) { scr_msgs::msg::ConfigUpdated config_updated_message; - config_updated_message.device = "preset"; + config_updated_message.device = device; config_updated_message.json = cfg.dump(); publishers.config_updated->publish(config_updated_message); } From 4a15d04a7dfa5933c870b0e8f33453507ca1cab0 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 10:53:54 -0500 Subject: [PATCH 099/113] Other UI changes --- display/index.html | 8 ++-- display/scripts/globals.js | 1 + display/scripts/main.js | 75 ++++++++++++++++++++++++++++---------- display/styles/index.css | 4 +- 4 files changed, 62 insertions(+), 26 deletions(-) diff --git a/display/index.html b/display/index.html index af9f77d2..54ba0f87 100644 --- a/display/index.html +++ b/display/index.html @@ -114,12 +114,12 @@

    Vision

    - - + +
    - - + +
    diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 2e3d45d7..dffd8daa 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -121,6 +121,7 @@ var addressKeys = { 5: "Misc 4", 6: "Misc 5", }, + "calculateWaypointDirection": "bool", "useOnlyWaypoints": "bool", "waypointDelay": "float", "vertical_fov": "float", diff --git a/display/scripts/main.js b/display/scripts/main.js index cc70b577..90572df4 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -911,12 +911,10 @@ $(document).ready(function () { const configElement = $("#options"); configElement.empty(); - for (const deviceId in config) { - const deviceConfig = config[deviceId]; - if (addressKeys[deviceId] == undefined) { - console.log(`Unknown Device Config: ${deviceId}`); - // const alert = $(``); - // configElement.append(alert); + // Sort the keys in each config by their addressKeys + for(const deviceId in addressKeys) + { + if (!(deviceId in config)) { continue; } @@ -925,31 +923,68 @@ $(document).ready(function () { deviceElement.append(`
    ${title}
    `); const deviceBody = $(`
    `); deviceElement.append(deviceBody); + + const deviceConfig = config[deviceId]; + for (const address in addressKeys[deviceId]) { + if (address == "internal_title") { + continue; + } - for (const address of Object.keys(deviceConfig).sort()) { - const data = deviceConfig[address]; - const type = addressKeys[deviceId][address]; - if (type == undefined) { - const alert = $(``); + if (!(address in deviceConfig)) { + const alert = $(``); deviceBody.append(alert); continue; } + const data = deviceConfig[address]; + const type = addressKeys[deviceId][address]; const inputElement = generateElementForConfiguration(data, type, deviceId, address); deviceBody.append(inputElement); } - for (const address in addressKeys[deviceId]) { - if (address in deviceConfig || address == "internal_title") { - continue; - } - - const alert = $(``); - deviceBody.append(alert); - } - configElement.append(deviceElement); } + + // config = outputConfig; + // for (const deviceId in config) { + // const deviceConfig = config[deviceId]; + // if (addressKeys[deviceId] == undefined) { + // console.log(`Unknown Device Config: ${deviceId}`); + // // const alert = $(``); + // // configElement.append(alert); + // continue; + // } + + // const title = addressKeys[deviceId]["internal_title"]; + // const deviceElement = $(`
    `); + // deviceElement.append(`
    ${title}
    `); + // const deviceBody = $(`
    `); + // deviceElement.append(deviceBody); + + // for (const address of Object.keys(deviceConfig).sort()) { + // const data = deviceConfig[address]; + // const type = addressKeys[deviceId][address]; + // if (type == undefined) { + // const alert = $(``); + // deviceBody.append(alert); + // continue; + // } + + // const inputElement = generateElementForConfiguration(data, type, deviceId, address); + // deviceBody.append(inputElement); + // } + + // for (const address in addressKeys[deviceId]) { + // if (address in deviceConfig || address == "internal_title") { + // continue; + // } + + // const alert = $(``); + // deviceBody.append(alert); + // } + + // configElement.append(deviceElement); + // } } function send(obj) { diff --git a/display/styles/index.css b/display/styles/index.css index 096c1464..db90d939 100644 --- a/display/styles/index.css +++ b/display/styles/index.css @@ -192,8 +192,8 @@ img[data-type="bigboy"] { } img[data-type="small"] { - width: 80px !important; - height: 80px !important; + width: 320px !important; + height: 320px !important; } #logging { From d0d75370a5f50b6ab8180cd052173b32914497bd Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 11:38:45 -0500 Subject: [PATCH 100/113] Add grids to astar/combination images --- autonav_ws/src/autonav_nav/src/astar.py | 4 ++++ autonav_ws/src/autonav_vision/src/combination.py | 6 ++++++ 2 files changed, 10 insertions(+) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 63042404..a32a7382 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -165,6 +165,10 @@ def create_path(self): cv2.circle(cvimg, (self.best_pos[0], self.best_pos[1]), 1, (255, 0, 0), 1) cvimg = cv2.resize(cvimg, (320, 320), interpolation=cv2.INTER_NEAREST) + # Draw a grid on the image that is the scale of the original image, so it should be a 80x80 grid scaled up 4x + for i in range(80): + cv2.line(cvimg, (0, i * 4), (320, i * 4), (85, 85, 85), 1) + cv2.line(cvimg, (i * 4, 0), (i * 4, 320), (85, 85, 85), 1) self.pathDebugImagePublisher.publish(CV_BRIDGE.cv2_to_compressed_imgmsg(cvimg)) def reconstruct_path(self, path, current): diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 22f7db8b..d319e8f5 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -91,6 +91,12 @@ def try_combine_grids(self): preview_image[i, j] = 0 if combined_grid.data[i * 80 + j] <= 10 else 255 preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB) preview_image = cv2.resize(preview_image, (320, 320)) + + # Draw a grid on the image that is the scale of the original image, so it should be a 80x80 grid scaled up 4x + for i in range(80): + cv2.line(preview_image, (0, i * 4), (320, i * 4), (85, 85, 85), 1) + cv2.line(preview_image, (i * 4, 0), (i * 4, 320), (85, 85, 85), 1) + compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) self.combined_grid_image_publisher.publish(compressed_image) From c35c002b7e5fd8795442bafa67dd612c1507f60b Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 15:26:07 -0500 Subject: [PATCH 101/113] Fixed a dropdown int issue in UI --- display/scripts/main.js | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/display/scripts/main.js b/display/scripts/main.js index 90572df4..5ae6c920 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -880,7 +880,7 @@ $(document).ready(function () { const select = document.createElement("select"); select.classList.add("form-select"); select.onchange = function () { - config[device][text] = select.value; + config[device][text] = parseInt(select.value); send({ op: "configuration", device: device, From b7c571b48cee58db2545d41ed5922042fe501358 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 15:26:15 -0500 Subject: [PATCH 102/113] Added new simulation waypoints --- autonav_ws/src/autonav_nav/src/astar.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index a32a7382..1b0c198a 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -33,8 +33,9 @@ ] simulation_waypoints = [ - [(35.19505, -97.43823), (35.19492, -97.43824), (35.19485, -97.43824), (35.19471, -97.43823)], # NORTH - [(35.19471, -97.43823), (35.19492, -97.43824), (35.19485, -97.43824), (35.19505, -97.43823)] # SOUTH + [(35.19510000, -97.43895000), (35.19491000, -97.43896000), (35.1948357, -97.43896), (35.19467540, -97.43895)], # NORTH + [(35.19467540, -97.43895), (35.1948357, -97.43896), (35.19491000, -97.43896000), (35.19510000, -97.43895000)], # SOUTH + [(35.194725, -97.43858), (35.1947823, -97.4387), (35.1948547, -97.43876), (35.1949272, -97.43867), (35.1950035, -97.43881)], # PRACTICE ] From 3d94b338d75d75c6380df289873499a5db8c6524 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 15:42:50 -0500 Subject: [PATCH 103/113] Added IGVC 2024 waypoints --- autonav_ws/src/autonav_nav/src/astar.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 1b0c198a..2e005964 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -23,13 +23,13 @@ CV_BRIDGE = cv_bridge.CvBridge() competition_waypoints = [ - [], # NORTH - [] # SOUTH + [(42.66827, -83.21934), (42.668121, -83.219361), (42.668077, -83.219359), (42.667928, -83.219328)], # NORTH + [(42.667928, -83.219328), (42.668077, -83.219359), (42.668121, -83.219361), (42.66827, -83.21934)] # SOUTH ] practice_waypoints = [ - [], # NORTH - [] # SOUTH + [(42.668212, -83.218459), (42.668086, -83.218446), (42.66796, -83.218433)], # FROM NORTH TO SOUTH + [(42.66796, -83.218433), (42.668086, -83.218446), (42.668212, -83.218459)] # FROM SOUTH TO NORTH ] simulation_waypoints = [ From af0b841bc9908074420960477830d9e7c9b8c2b1 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 16:36:03 -0500 Subject: [PATCH 104/113] Fixed autobuild issue (setup) --- setup/setup.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/setup/setup.sh b/setup/setup.sh index 50a1da4d..f82ca0c0 100755 --- a/setup/setup.sh +++ b/setup/setup.sh @@ -14,10 +14,10 @@ sudo apt update sudo apt install wget unzip -y # Vectornav Dependencies -bash vnav.sh +bash etc/vnav.sh # Steam Controller Dependencies -bash steam.sh +bash etc/steam.sh # Python deps sudo apt install python3-pip -y From f872c0d7e1293b20c97ddd99f69fc45d457c77cf Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 16:50:01 -0500 Subject: [PATCH 105/113] Try to auto determine video index --- autonav_ws/src/autonav_serial/src/camera.py | 23 +++++++++++++++++++-- 1 file changed, 21 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index cacfd10c..11ff9fba 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -11,6 +11,7 @@ from scr.states import DeviceStateEnum, SystemStateEnum import os import json +import subprocess bridge = CvBridge() @@ -112,8 +113,26 @@ def camera_worker(self): def main(): rclpy.init() - node_left = CameraNode("left", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_8174526F-video-index0") - node_right = CameraNode("right", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_3F47331F-video-index0") + + # Check whether index0 or index1 is the actual footage. Use V4L2-CTL to check if "exposure_time_absolute" exists + # sudo v4l2-ctl --device={x} --all + left_index0 = subprocess.run(["v4l2-ctl", "--device=0", "--all"], stdout=subprocess.PIPE) + right_index0 = subprocess.run(["v4l2-ctl", "--device=2", "--all"], stdout=subprocess.PIPE) + + correct_left = None + correct_right = None + if "exposure_time_absolute" in left_index0.stdout.decode("utf-8"): + correct_left = 0 + else: + correct_left = 1 + + if "exposure_time_absolute" in right_index0.stdout.decode("utf-8"): + correct_right = 0 + else: + correct_right = 1 + + node_left = CameraNode("left", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_8174526F-video-index" + str(correct_left)) + node_right = CameraNode("right", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_3F47331F-video-index" + str(correct_right)) Node.run_nodes([node_left, node_right]) rclpy.shutdown() From 4adc45008e033e926204f44299b661a600bbdadf Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 21 May 2024 17:11:29 -0500 Subject: [PATCH 106/113] Test stuff --- autonav_ws/src/autonav_launch/launch/competition.xml | 6 ++++-- autonav_ws/src/autonav_nav/src/astar.py | 3 ++- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/competition.xml b/autonav_ws/src/autonav_launch/launch/competition.xml index 33e17eb1..b9ceedf0 100644 --- a/autonav_ws/src/autonav_launch/launch/competition.xml +++ b/autonav_ws/src/autonav_launch/launch/competition.xml @@ -5,8 +5,10 @@ - - + + + + diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 2e005964..ca56d0cc 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -24,7 +24,8 @@ competition_waypoints = [ [(42.66827, -83.21934), (42.668121, -83.219361), (42.668077, -83.219359), (42.667928, -83.219328)], # NORTH - [(42.667928, -83.219328), (42.668077, -83.219359), (42.668121, -83.219361), (42.66827, -83.21934)] # SOUTH + [(42.667928, -83.219328), (42.668077, -83.219359), (42.668121, -83.219361), (42.66827, -83.21934)], # SOUTH + [(35.210475, -97.441927), (35.210476, -97.442321), (35.210599, -97.442323), (35.210599, -97.442096)], # NORMAN TEST WAYPOINTS ] practice_waypoints = [ From 753833f7e0ccaf1a821dfe62a0dd0385f0f411bf Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 22 May 2024 08:49:40 -0500 Subject: [PATCH 107/113] Hide IMU section on GUI --- display/index.html | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/display/index.html b/display/index.html index 54ba0f87..0945a5ca 100644 --- a/display/index.html +++ b/display/index.html @@ -84,14 +84,14 @@
    Is Fixed:
    Satellites:
    -
    +

    Path Planning

    From 72ac504a355b1586c1cfdecc63945e7893f6e576 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 22 May 2024 08:49:53 -0500 Subject: [PATCH 108/113] Fix waypoints starting at wrong time --- autonav_ws/src/autonav_nav/src/astar.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index ca56d0cc..fd324082 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -97,7 +97,7 @@ def init(self): self.map_timer = self.create_timer(0.1, self.create_path) self.reset_at = -1.0 - self.waypoint_start_time = time.time() + self.config.waypointDelay + self.waypoints = [] self.set_device_state(DeviceStateEnum.OPERATING) def getAngleDifference(self, to_angle, from_angle): @@ -112,7 +112,7 @@ def onReset(self): self.cost_map = None self.best_pos = (0, 0) self.waypoints = [] - self.waypoint_start_time = self.config.waypointDelay + time.time() + self.waypoint_start_time = 0 def get_waypoints_for_dir(self): if not self.config.calculateWaypointDirection: From 4ebc160c8c186e2e53aa9351ac32092d6c6f3335 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 22 May 2024 08:50:02 -0500 Subject: [PATCH 109/113] Mobi-stop now stops the robot --- autonav_ws/src/autonav_nav/src/path_resolver.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/autonav_ws/src/autonav_nav/src/path_resolver.py b/autonav_ws/src/autonav_nav/src/path_resolver.py index 09de5c35..48973770 100644 --- a/autonav_ws/src/autonav_nav/src/path_resolver.py +++ b/autonav_ws/src/autonav_nav/src/path_resolver.py @@ -87,6 +87,13 @@ def system_state_transition(self, old: SystemState, updated: SystemState): inputPacket.angular_velocity = 0.0 self.motor_publisher.publish(inputPacket) + if updated.mobility == False and self.device_state == DeviceStateEnum.OPERATING and (old.mobility == True or updated.mobility == True): + self.set_device_state(DeviceStateEnum.READY) + inputPacket = MotorInput() + inputPacket.forward_velocity = 0.0 + inputPacket.angular_velocity = 0.0 + self.motor_publisher.publish(inputPacket) + if updated.state == SystemStateEnum.AUTONOMOUS and self.device_state == DeviceStateEnum.OPERATING and updated.mobility == False: self.safety_lights_publisher.publish(toSafetyLights(False, False, 2, 255, "#00A36C")) From 068db90825e245b4fbbc32ddb0e66e885aa80611 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 22 May 2024 08:50:46 -0500 Subject: [PATCH 110/113] Serial node now zeros motors when switching states/mobility --- .../src/autonav_serial/src/serial_node.py | 29 +++++++++++++++---- 1 file changed, 24 insertions(+), 5 deletions(-) diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index 59d03808..5b3f7ecd 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -8,7 +8,7 @@ import struct from autonav_msgs.msg import MotorInput, MotorFeedback, MotorControllerDebug, SafetyLights, Conbus from scr.node import Node -from scr.states import DeviceStateEnum +from scr.states import DeviceStateEnum, SystemStateEnum, SystemState MOTOR_CONTROL_ID = 10 @@ -60,6 +60,27 @@ def init(self): self.canReadThread.daemon = True self.canReadThread.start() + def zero_motors(self): + packed_data = struct.pack("hh", int(0 * 1000.0), int(0 * 1000.0)) + can_msg = can.Message(arbitration_id=MOTOR_CONTROL_ID, data=packed_data) + try: + self.can.send(can_msg) + except can.CanError: + pass + + def system_state_transition(self, old: SystemState, updated: SystemState): + if old.state != SystemStateEnum.DISABLED and updated.state == SystemStateEnum.DISABLED: + self.zero_motors() + + if old.state == SystemStateEnum.AUTONOMOUS and updated.state == SystemStateEnum.MANUAL: + self.zero_motors() + + if old.state == SystemStateEnum.MANUAL and updated.state == SystemStateEnum.AUTONOMOUS: + self.zero_motors() + + if old.mobility == True and updated.mobility == False: + self.zero_motors() + def canThreadWorker(self): while rclpy.ok(): if self.device_state != DeviceStateEnum.READY and self.device_state != DeviceStateEnum.OPERATING: @@ -182,10 +203,8 @@ def onMotorInputReceived(self, input: MotorInput): if self.device_state != DeviceStateEnum.OPERATING: return - packed_data = struct.pack("hh", int( - input.forward_velocity * 1000.0), int(input.angular_velocity * 1000.0)) - can_msg = can.Message( - arbitration_id=MOTOR_CONTROL_ID, data=packed_data) + packed_data = struct.pack("hh", int(input.forward_velocity * 1000.0), int(input.angular_velocity * 1000.0)) + can_msg = can.Message(arbitration_id=MOTOR_CONTROL_ID, data=packed_data) try: self.can.send(can_msg) except can.CanError: From 40bf5d543feef76b519311002d1c1a77fd46106c Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 22 May 2024 08:52:49 -0500 Subject: [PATCH 111/113] Misc1/Practice is now an empty set of waypoints --- autonav_ws/src/autonav_nav/src/astar.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index fd324082..25f46dfc 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -30,7 +30,8 @@ practice_waypoints = [ [(42.668212, -83.218459), (42.668086, -83.218446), (42.66796, -83.218433)], # FROM NORTH TO SOUTH - [(42.66796, -83.218433), (42.668086, -83.218446), (42.668212, -83.218459)] # FROM SOUTH TO NORTH + [(42.66796, -83.218433), (42.668086, -83.218446), (42.668212, -83.218459)], # FROM SOUTH TO NORTH + [] # EMPTY ] simulation_waypoints = [ From 451ef09dfa7e498a83f3e1c6421462a657cc8845 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 22 May 2024 10:39:25 -0500 Subject: [PATCH 112/113] UI now uses raw image instead of base64 --- autonav_ws/src/autonav_display/src/display.py | 32 ++++++++-------- display/index.html | 2 +- display/scripts/main.js | 38 +++---------------- display/scripts/utilities.js | 11 +++++- 4 files changed, 33 insertions(+), 50 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 43883158..2d3beb07 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -14,8 +14,11 @@ import json import base64 import time +import cv_bridge +import cv2 async_loop = asyncio.new_event_loop() +bridge = cv_bridge.CvBridge() class Limiter: @@ -56,21 +59,18 @@ def __init__(self): # Limiter self.limiter = Limiter() - self.limiter.setLimit("/autonav/MotorInput", 5) + self.limiter.setLimit("/autonav/MotorInput", 2) self.limiter.setLimit("/autonav/MotorFeedback", 5) self.limiter.setLimit("/autonav/MotorControllerDebug", 1) - self.limiter.setLimit("/autonav/imu", 5) - self.limiter.setLimit("/autonav/gps", 5) - self.limiter.setLimit("/autonav/position", 5) - self.limiter.setLimit("/autonav/camera/compressed/left", 2) - self.limiter.setLimit("/autonav/camera/compressed/right", 2) - self.limiter.setLimit("/autonav/cfg_space/raw/image/left", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/image/left_small", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/image/right_small", 5) - self.limiter.setLimit("/autonav/cfg_space/combined/image", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) - self.limiter.setLimit("/autonav/debug/astar/image", 5) + self.limiter.setLimit("/autonav/imu", 1) + self.limiter.setLimit("/autonav/gps", 3) + self.limiter.setLimit("/autonav/position", 3) + self.limiter.setLimit("/autonav/camera/compressed/left", 4) + self.limiter.setLimit("/autonav/camera/compressed/right", 4) + self.limiter.setLimit("/autonav/cfg_space/raw/image/left_small", 4) + self.limiter.setLimit("/autonav/cfg_space/raw/image/right_small", 4) + self.limiter.setLimit("/autonav/cfg_space/combined/image", 4) + self.limiter.setLimit("/autonav/debug/astar/image", 4) self.limiter.setLimit("/autonav/debug/astar", 5) # Clients @@ -127,14 +127,14 @@ def push_image(self, topic, msg): if not self.limiter.use(topic): return - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") + cvimg = bridge.compressed_imgmsg_to_cv2(msg) + _, img = cv2.imencode('.jpg', cvimg) self.push_old(json.dumps({ "op": "data", "topic": topic, "format": msg.format, - "data": base64_str + "data": list(img.tobytes()) })) def push(self, topic, data, unique_id=None): diff --git a/display/index.html b/display/index.html index 0945a5ca..09d10a73 100644 --- a/display/index.html +++ b/display/index.html @@ -119,7 +119,7 @@

    Vision

    - +
    diff --git a/display/scripts/main.js b/display/scripts/main.js index 5ae6c920..0010f1d1 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -433,58 +433,32 @@ $(document).ready(function () { } if (topic == "/autonav/camera/compressed/left") { - // Set to - const imgElement = document.getElementById("target_raw_camera_left"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; + transferImageToElement("target_raw_camera_left", msg.data); return; } if (topic == "/autonav/camera/compressed/right") { - // Set to - const imgElement = document.getElementById("target_raw_camera_right"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; - return; - } - - if (topic == "/autonav/cfg_space/raw/image/left") { - const imgElement = document.getElementById("target_filtered_camera_left"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; - return; - } - - if (topic == "/autonav/cfg_space/raw/image/right") { - const imgElement = document.getElementById("target_filtered_camera_right"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; + transferImageToElement("target_raw_camera_right", msg.data); return; } if (topic == "/autonav/cfg_space/raw/image/left_small") { - const imgElement = document.getElementById("target_filtered_left"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; + transferImageToElement("target_filtered_left", msg.data); return; } if (topic == "/autonav/cfg_space/raw/image/right_small") { - const imgElement = document.getElementById("target_filtered_right"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; + transferImageToElement("target_filtered_right", msg.data); return; } if (topic == "/autonav/cfg_space/combined/image") { - const imgElement = document.getElementById("target_combined"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; - return; - } - - if (topic == "/autonav/cfg_space/raw/debug") { - const imgElement = document.getElementById("target_camera_raw"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; + transferImageToElement("target_combined", msg.data); return; } if (topic == "/autonav/debug/astar/image") { - const imgElement = document.getElementById("target_expandified"); - imgElement.src = `data:image/jpeg;base64,${msg.data}`; + transferImageToElement("target_astar", msg.data); return; } diff --git a/display/scripts/utilities.js b/display/scripts/utilities.js index ad9fef1c..0108877c 100644 --- a/display/scripts/utilities.js +++ b/display/scripts/utilities.js @@ -23,11 +23,20 @@ const fromBytesToBool = (bytes) => { return getBytesView(bytes).getUint8(0) == 1; } -const transferImageToElement = (id, data) => { +const transferImageToElementOld = (id, data) => { const img = document.getElementById(id); img.src = "data:image/jpeg;base64," + data; } +const transferImageToElement = (id, data) => { + const img = document.getElementById(id); + const uint8Array = new Uint8Array(data); + const blob = new Blob([uint8Array], { type: "image/jpeg" }); + const urlCreator = window.URL || window.webkitURL; + const imageUrl = urlCreator.createObjectURL(blob); + img.src = imageUrl; +} + const trasferImageBytesToElement = (id, data) => { const img = document.getElementById(id); const msgarray = new Uint8Array(data); From 8d8da8b5a33d6fcdc1fd6205b87aad3e4619fd91 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Wed, 22 May 2024 15:59:47 -0500 Subject: [PATCH 113/113] scrabby is actually capable of pf now --- autonav_ws/src/autonav_filters/src/filters.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/autonav_ws/src/autonav_filters/src/filters.py b/autonav_ws/src/autonav_filters/src/filters.py index ca1c0fd9..69f885ad 100644 --- a/autonav_ws/src/autonav_filters/src/filters.py +++ b/autonav_ws/src/autonav_filters/src/filters.py @@ -97,10 +97,6 @@ def onMotorFeedbackReceived(self, msg: MotorFeedback): position.latitude = gps_x position.longitude = gps_y - if self.system_mode == SystemModeEnum.SIMULATION and self.last_gps is not None: - position.latitude = self.last_gps.latitude - position.longitude = self.last_gps.longitude - self.position_publisher.publish(position)