diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..0e89694 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +CLAUDE.md +CODEX.md +__pycache__/ +*.pyc diff --git a/Arduino/armMotors/armMotors.ino b/Arduino/armMotors/armMotors.ino new file mode 100644 index 0000000..831d8ed --- /dev/null +++ b/Arduino/armMotors/armMotors.ino @@ -0,0 +1,256 @@ +/******************************************************************************* +* Copyright 2016 ROBOTIS CO., LTD. +* +* Licensed under the Apache License, Version 2.0 (the "License"); +* you may not use this file except in compliance with the License. +* You may obtain a copy of the License at +* +* http://www.apache.org/licenses/LICENSE-2.0 +* +* Unless required by applicable law or agreed to in writing, software +* distributed under the License is distributed on an "AS IS" BASIS, +* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +* See the License for the specific language governing permissions and +* limitations under the License. +*******************************************************************************/ + +//====================================================================================== + +// id of OpenCR board : usb-ROBOTIS_OpenCR_Virtual_ComPort_in_FS_Mode_FFFFFFFEFFFF-if00 + +//====================================================================================== + +#include +#include +#include +#include +#include +#include +#include + +// For OpenCR, there is a DXL Power Enable pin, so you must initialize and control it. +// Reference link : https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/DynamixelSDK/src/dynamixel_sdk/port_handler_arduino.cpp#L78 +#define DXL_SERIAL Serial3 +#define DEBUG_SERIAL Serial +const int DXL_DIR_PIN = 84; // OpenCR Board's DIR PIN. + +#define SERVO_SERIAL Serial1 +#define BAUDRATE 1000000 +#define SERVO_ID_4 0x06 +#define SERVO_ID_5 0x07 +#define SERVO_ID_6 0x08 +#define SERVO_ID_3 0x02 +//-------------------------------------------------------------------------------------- + +// the id of motors that are close to each other are 1 and 2 and the top motor id is 3 + +//-------------------------------------------------------------------------------------- +const uint8_t DXL_ID_1 = 1; +const uint8_t DXL_ID_2 = 2; +const uint8_t DXL_ID_3 = 3; +static const uint8_t MAX_MOTORS = 5; +int16_t commanded[MAX_MOTORS]; + +const float DXL_PROTOCOL_VERSION = 2.0; + +Dynamixel2Arduino dxl(DXL_SERIAL, DXL_DIR_PIN); + +SMS_STS st; + +//This namespace is required to use Control table item names +using namespace ControlTableItem; + +long currentPositionM1 = 0; +long currentPositionM2 = 0; +long currentPositionM3 = 0; +long currentPositionM4 = 0; +long currentPositionM5 = 0; +long currentPositionM6 = 0; +long returnedPostions[MAX_MOTORS]; +float tmp[MAX_MOTORS] = {0.0,0.0,0.0,0.0,0.0}; + +void initializeDxlMotors() { + dxl.begin(57600); + + uint8_t ids[] = {DXL_ID_1, DXL_ID_2}; + for(int i=0; i<2; i++) { + dxl.torqueOff(ids[i]); + dxl.setOperatingMode(ids[i], OP_EXTENDED_POSITION); + dxl.writeControlTableItem(PROFILE_ACCELERATION, ids[i], 15); + dxl.writeControlTableItem(PROFILE_VELOCITY, ids[i], 40); + dxl.torqueOn(ids[i]); + } + // Configure Motor 3 + dxl.torqueOff(DXL_ID_3); + dxl.setOperatingMode(DXL_ID_3, OP_EXTENDED_POSITION); + dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID_3, 15); + dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID_3, 40); + dxl.torqueOn(DXL_ID_3); +} + +void recoverSTMotors() { + st.EnableTorque(SERVO_ID_4, 1); + st.EnableTorque(SERVO_ID_5, 1); + st.EnableTorque(SERVO_ID_6, 1); +} + +// Generic conversion function +int32_t radToPos(float radians, int resolution) { + return (int32_t)((radians * resolution) / (2.0 * PI)); +} +void checkMotorHealth() { + for(uint8_t id = 1; id <= 3; id++) { + // Read Temperature (Celsius) + int16_t temp = dxl.readControlTableItem(PRESENT_TEMPERATURE, id); + // Read Load (0 to 1000 range, bit 10 is direction) + int16_t load = dxl.readControlTableItem(PRESENT_LOAD, id); + + DEBUG_SERIAL.print("ID: "); DEBUG_SERIAL.print(id); + DEBUG_SERIAL.print(" | Temp: "); DEBUG_SERIAL.print(temp); + DEBUG_SERIAL.print("C | Load: "); DEBUG_SERIAL.println(load); + + if (temp > 65) { + DEBUG_SERIAL.println("!!! WARNING: MOTOR OVERHEATING !!!"); + } + } +} +void safeReboot(uint8_t id) { + int16_t temp = dxl.readControlTableItem(PRESENT_TEMPERATURE, id); + + if (temp < 60) { // Only reboot if it's cooled down a bit + dxl.reboot(id); + delay(500); + initializeDxlMotors(); + } else { + } +} +void setup() { + // Use UART port of DYNAMIXEL Shield to debug. + DEBUG_SERIAL.begin(115200); + SERVO_SERIAL.begin(BAUDRATE); + while(!DEBUG_SERIAL); + + st.pSerial = &SERVO_SERIAL; + // Set Port baudrate to 57600bps. This has to match with DYNAMIXEL baudrate. + dxl.begin(57600); + // Set Port Protocol Version. This has to match with DYNAMIXEL protocol version. + dxl.setPortProtocolVersion(DXL_PROTOCOL_VERSION); + + // Get DYNAMIXEL information for both motors + dxl.ping(DXL_ID_1); + dxl.ping(DXL_ID_2); + dxl.ping(DXL_ID_3); + // Configure Motor 1, 2 + uint8_t ids[] = {DXL_ID_1, DXL_ID_2}; + for(int i=0; i<2; i++) { + dxl.torqueOff(ids[i]); + dxl.setOperatingMode(ids[i], OP_EXTENDED_POSITION); + dxl.writeControlTableItem(PROFILE_ACCELERATION, ids[i], 15); + dxl.writeControlTableItem(PROFILE_VELOCITY, ids[i], 50); // 3rd value is speed, 0 disable this value, making it go as fast as possible + dxl.torqueOn(ids[i]); + } + // Configure Motor 3 + dxl.torqueOff(DXL_ID_3); + dxl.setOperatingMode(DXL_ID_3, OP_EXTENDED_POSITION); + dxl.writeControlTableItem(PROFILE_ACCELERATION, DXL_ID_3, 15); + dxl.writeControlTableItem(PROFILE_VELOCITY, DXL_ID_3, 50); + dxl.torqueOn(DXL_ID_3); + + +} + +unsigned long lastCheck = 0; + +void loop() { + // if dxl motor lost his power 0.0:0.0:0.0:0.0:0.0 + if (dxl.ping(DXL_ID_1) == false) { + delay(1000); + initializeDxlMotors(); + return; + } + // if st motor lost his power + if (st.ReadSpeed(SERVO_ID_4) == -1) { + delay(500); + recoverSTMotors(); + } + + // Read from DEBUG_SERIAL, which is the connection to your computer/serial monitor + String rx = DEBUG_SERIAL.readStringUntil('\n'); + + sscanf(rx.c_str(), "%f:%f:%f:%f:%f:", &tmp[0], &tmp[1], &tmp[2], &tmp[3], &tmp[4]); + + commanded[0] = radToPos(tmp[0] , 4096); + commanded[1] = radToPos(tmp[1] , 4096); + commanded[2] = radToPos(tmp[2] , 4096); + commanded[3] = radToPos(tmp[3] , 4096); + commanded[4] = radToPos(tmp[4] , 4096); + + currentPositionM1 = commanded[0]; + currentPositionM2 = 3 * commanded[1] ; + currentPositionM3 = -3 * commanded[1] + 4096; + + long m4_pos = -2 * commanded[2]; + m4_pos = m4_pos % 4096; + if(m4_pos < 0) { + m4_pos += 4096; + } + currentPositionM4 = m4_pos; + + currentPositionM5 = commanded[3]; + currentPositionM6 = commanded[4]; + + + // Send commands to motors + dxl.setGoalPosition(DXL_ID_1, currentPositionM2); + dxl.setGoalPosition(DXL_ID_2, currentPositionM3); + dxl.setGoalPosition(DXL_ID_3, currentPositionM4); + st.WritePosEx(SERVO_ID_3, currentPositionM4, 100, 0); + //WritePosEx(ID, Position, Speed, Acceleration) 0 for max value + st.WritePosEx(SERVO_ID_4, currentPositionM6, 100, 0); + st.WritePosEx(SERVO_ID_5, currentPositionM5, 100, 0); + st.WritePosEx(SERVO_ID_6, currentPositionM1, 400, 0); + + // Format: D1:D3:S4:S5:S6 + int dxl1_pos = (int) dxl.getPresentPosition(DXL_ID_1) / 3; + int raw_servo3_pos = (int) st.ReadPos(SERVO_ID_3); + int signed_servo3_pos = raw_servo3_pos; + if (signed_servo3_pos > 2048) { + signed_servo3_pos -= 4096; + } + int dxl3_pos = signed_servo3_pos / -2; + String line = ""; + + line += String(st.ReadPos(SERVO_ID_6)); + line += ":"; + line += String(dxl1_pos); + line += ":"; + line += String(dxl3_pos); + line += ":"; + line += String(st.ReadPos(SERVO_ID_5)); + line += ":"; + line += String(st.ReadPos(SERVO_ID_4)); + + DEBUG_SERIAL.println(line); + +/* + DEBUG_SERIAL.print(st.ReadPos(SERVO_ID_6)); + DEBUG_SERIAL.print(":"); + DEBUG_SERIAL.print(dxl1_pos); + DEBUG_SERIAL.print(":"); + DEBUG_SERIAL.print(dxl3_pos); + DEBUG_SERIAL.print(":"); + DEBUG_SERIAL.print(st.ReadPos(SERVO_ID_5)); + DEBUG_SERIAL.print(":"); + DEBUG_SERIAL.println(st.ReadPos(SERVO_ID_4)); +*/ +// --------- reboot the motor if it shutdown ------------ +/* +for(uint8_t id = 1; id <= 3; id++) { + uint8_t error = dxl.readControlTableItem(HARDWARE_ERROR_STATUS, id); + if(error != 0) { + safeReboot(id); + } + } +} +*/ +} diff --git a/Arduino/serial-flipper_1-3/serial-flipper_1-3.ino b/Arduino/serial-flipper_1-3/serial-flipper_1-3.ino new file mode 100644 index 0000000..b79533d --- /dev/null +++ b/Arduino/serial-flipper_1-3/serial-flipper_1-3.ino @@ -0,0 +1,197 @@ +#include +//Baudrate is defined in the TLE Library + +// Shield-Gruppenobjekt +TLE9879_Group *shields; + +// FrontLeft_Board = BOARD4; +// FrontRight_Board = BOARD1; +// RearLeft_Board = BOARD3; +// RearRight_Board = BOARD2; + +//variables for connection +int16_t stop_delay = 10; +int16_t refresh_rate = 100; //[Hz] +int16_t DELAY = (1000 / refresh_rate) - stop_delay; //delay between two loops according to refresh_rate + +//variables for movement controll +int16_t FrontLeft = 0; +int16_t FrontRight = 0; +int16_t RearLeft = 0; +int16_t RearRight = 0; +int16_t default_speed = 2900; + +int16_t FrontLeft_mode = 0; +int16_t FrontRight_mode = 0; +int16_t RearLeft_mode = 0; +int16_t RearRight_mode = 0; + +void setup(){ + + // Initialize the Shield group object with the number of Shields in the stack + shields = new TLE9879_Group(4); + + // Override serial baud rate to 115200 to match ROS serial_comms.hpp + // Also set a low timeout so readStringUntil doesn't block for 1000ms on fragmented data + Serial.begin(115200); + Serial.setTimeout(10); + + // Set the desired mode (FOC, HALL, BEMF) + shields->setMode(HALL, BOARD4); + shields->setMode(HALL, BOARD3); + shields->setMode(HALL, BOARD2); + shields->setMode(HALL, BOARD1); + + //Set Hall Frequency + shields->setParameter(HALL_PWM_FREQ, 20000, BOARD4); //[kHz] + shields->setParameter(HALL_PWM_FREQ, 20000, BOARD3); //[kHz] + shields->setParameter(HALL_PWM_FREQ, 20000, BOARD2); //[kHz] + shields->setParameter(HALL_PWM_FREQ, 20000, BOARD1); //[kHz] + + //set number of pole pairs (look up in data sheet) + shields->setParameter(HALL_POLE_PAIRS, 2, BOARD4); + shields->setParameter(HALL_POLE_PAIRS, 2, BOARD3); + shields->setParameter(HALL_POLE_PAIRS, 2, BOARD2); + shields->setParameter(HALL_POLE_PAIRS, 2, BOARD1); + + //set minimum I value (must be 0 to enable motor speed = 0) + shields->setParameter(HALL_SPEED_IMIN, 0, BOARD4); + shields->setParameter(HALL_SPEED_IMIN, 0, BOARD3); + shields->setParameter(HALL_SPEED_IMIN, 0, BOARD2); + shields->setParameter(HALL_SPEED_IMIN, 0, BOARD1); + + //set max I value (indirectly restricts max current) + shields->setParameter(HALL_SPEED_IMAX, 29, BOARD4); + shields->setParameter(HALL_SPEED_IMAX, 29, BOARD3); + shields->setParameter(HALL_SPEED_IMAX, 29, BOARD2); + shields->setParameter(HALL_SPEED_IMAX, 29, BOARD1); + + //set min P value (must be 0 to enable motor speed = 0) + shields->setParameter(HALL_SPEED_PIMIN, 0, BOARD4); + shields->setParameter(HALL_SPEED_PIMIN, 0, BOARD3); + shields->setParameter(HALL_SPEED_PIMIN, 0, BOARD2); + shields->setParameter(HALL_SPEED_PIMIN, 0, BOARD1); + + //set max P value (indirectly restricts max current) + shields->setParameter(HALL_SPEED_PIMAX, 29, BOARD4); + shields->setParameter(HALL_SPEED_PIMAX, 29, BOARD3); + shields->setParameter(HALL_SPEED_PIMAX, 29, BOARD2); + shields->setParameter(HALL_SPEED_PIMAX, 29, BOARD1); + + /* + //proportional gain in the PI controller formula + shields->setParameter(HALL_SPEED_KP, 500, BOARD4); + + //integral gain in the PI controller formula + shields->setParameter(HALL_SPEED_KI, 100, BOARD4); + + //shields->setLed(LED_ON, BOARD4); + shields->setLedColor(COLOR_BLUE, BOARD4); + */ + + shields->setMotorSpeed(0, BOARD4); + shields->setMotorSpeed(0, BOARD3); + shields->setMotorSpeed(0, BOARD2); + shields->setMotorSpeed(0, BOARD1); + + shields->setMotorMode(START_MOTOR, BOARD4); + shields->setMotorMode(START_MOTOR, BOARD3); + shields->setMotorMode(START_MOTOR, BOARD2); + shields->setMotorMode(START_MOTOR, BOARD1); + +} + // Helper function to figure out if the motor is moving forward, backward, or stopped +int8_t getSign(int16_t val) { + if (val > 0) return 1; + if (val < 0) return -1; + return 0; +} + +void loop() { + bool newData = false; + + // 1. FLUSH THE SERIAL BUFFER (Fixes continuous command lag) + // Read all pending messages, but only keep the absolute newest one. + if (Serial.available() > 0) { + String rx_msg = ""; + while (Serial.available() > 0) { + String temp = Serial.readStringUntil('\n'); + if (temp.length() > 5) { // Basic check to ensure it's a valid string + rx_msg = temp; + } + } + + // Process only the freshest command + if (rx_msg != "") { + int tempFL = 0, tempFR = 0, tempRL = 0, tempRR = 0; + + // sscanf returns the number of items it successfully matched + int parsed = sscanf(rx_msg.c_str(), "FL%iFR%iRL%iRR%i", &tempFL, &tempFR, &tempRL, &tempRR); + + // Only apply the changes if ALL 4 values were successfully read! + if (parsed == 4) { + FrontLeft = tempFL; + FrontRight = tempFR; + // Flip the rears immediately upon successful read + RearLeft = tempRL * -1; + RearRight = tempRR * -1; + newData = true; + } else { + // If we got a partial message, we just ignore it. + // A full message will arrive in the next millisecond anyway. + newData = false; + } + } + } + + // 2. PROCESS MOTORS ONLY IF WE HAVE NEW DATA + if (newData) { + bool changeFL = (FrontLeft != FrontLeft_mode); + bool changeFR = (FrontRight != FrontRight_mode); + bool changeRL = (RearLeft != RearLeft_mode); + bool changeRR = (RearRight != RearRight_mode); + + // Check if the change is an actual DIRECTION change (or starting from 0) + bool reverseFL = changeFL && (getSign(FrontLeft) != getSign(FrontLeft_mode)); + bool reverseFR = changeFR && (getSign(FrontRight) != getSign(FrontRight_mode)); + bool reverseRL = changeRL && (getSign(RearLeft) != getSign(RearLeft_mode)); + bool reverseRR = changeRR && (getSign(RearRight) != getSign(RearRight_mode)); + + // 3. APPLY STOPS ONLY FOR DIRECTION CHANGES + bool needs_delay = false; + if (reverseFL) { shields->setMotorMode(STOP_MOTOR, BOARD4); needs_delay = true; } + if (reverseFR) { shields->setMotorMode(STOP_MOTOR, BOARD1); needs_delay = true; } + if (reverseRL) { shields->setMotorMode(STOP_MOTOR, BOARD3); needs_delay = true; } + if (reverseRR) { shields->setMotorMode(STOP_MOTOR, BOARD2); needs_delay = true; } + + // Wait ONCE, and only if a motor is actually changing direction + if (needs_delay) { + delay(stop_delay); + } + + // 4. UPDATE SPEEDS AND RESTART + if (changeFL) { + shields->setMotorSpeed(FrontLeft * default_speed, BOARD4); + if (reverseFL) shields->setMotorMode(START_MOTOR, BOARD4); + FrontLeft_mode = FrontLeft; + } + if (changeFR) { + shields->setMotorSpeed(FrontRight * default_speed, BOARD1); + if (reverseFR) shields->setMotorMode(START_MOTOR, BOARD1); + FrontRight_mode = FrontRight; + } + if (changeRL) { + shields->setMotorSpeed(RearLeft * default_speed, BOARD3); + if (reverseRL) shields->setMotorMode(START_MOTOR, BOARD3); + RearLeft_mode = RearLeft; + } + if (changeRR) { + shields->setMotorSpeed(RearRight * default_speed, BOARD2); + if (reverseRR) shields->setMotorMode(START_MOTOR, BOARD2); + RearRight_mode = RearRight; + } + } + + // 5. Delay until next cycle + delay(DELAY); +} \ No newline at end of file diff --git a/LICENSE b/LICENSE deleted file mode 100644 index f45914e..0000000 --- a/LICENSE +++ /dev/null @@ -1,21 +0,0 @@ -MIT License - -Copyright (c) 2023 ResQBot - -Permission is hereby granted, free of charge, to any person obtaining a copy -of this software and associated documentation files (the "Software"), to deal -in the Software without restriction, including without limitation the rights -to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -copies of the Software, and to permit persons to whom the Software is -furnished to do so, subject to the following conditions: - -The above copyright notice and this permission notice shall be included in all -copies or substantial portions of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -SOFTWARE. diff --git a/README.md b/README.md index 42f651d..7e90058 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,1312 @@ -# resqbot_dev +# Res.Q Bots Setup +Dieses Tutorial ist eine Schritt-für-Schritt Anleitung, um die komplette Softwaresuite des Res.Q Bots Teams einzurichten und mit dem Coden zu starten. Es werden keinerlei Vorkenntnisse benötigt. -used packages: -joy -usb_cam -image_transport + + +**Disclaimer:** +This REEADME is meant for internal use by Res.Q Bots members. While we do not forbid anyone else from using it, we want to caution you, that this file is not complete and updated infrequently. It is by no means a reliable source to work with, if you are not in direct contact with one of the people who have been working on it. Most of the packages used are open source packages developed by other people, who know what they are doing way better than us. +We have done our best to link to the original GitHubs for any non standard ROS packages. + + + + + +  +# Inhalt +[Betriebssystem](#betriebssystem) +[Ubuntu Grundlagen](#ubuntu-terminals) +     [Ubuntu Terminal Befehle](#wichtige-befehle) +     [ROS2 Befehle](#ros2-befehle) +     [Nützliche Programme](#nützliche-programme) +[ROS2 Installieren](#ros2-installieren) +[ROS2 Packages installieren und einrichten](#ros2-packages-installieren-und-einrichten) +     [Package Liste](#package-liste) +     [Steuerung](#steuerung) +     [Sensorik](#sensorik) +     [Lidar](#lidar-livox-mid-360) +     [Anzeige](#anzeige) +     [Development-Tools](#development-Tools) +[Packages entwickeln](#packages-entwickeln) +     [Grundlagen](#grundlagen) +     [ros2_control Packages](#ros2_control-packages) +     []() +     []() + + + + + +  +  +  +  +# Betriebssystem +Aktuell verwenden wir **Ubuntu 24.04 LTS** als OS, da dies die Basis für **ROS2 Jazzy** bildet. + +### Voraussetzungen +Internetverbindung +USB Stick mit min 8 GB Speicher + + +### Installation +Zuerst muss das **Ubuntu 24.04 Desktop** Image von der Ubuntu Website heruntergeladen werden. +Danach ein Tool zum Erstellen eines USB-Startermediums installieren. z.B. **Rufus** oder **Balena-Etcher** +Diese Tools haben auf ihren Webseites normalerweise Anleitungen, wie sie zu verwenden sind. Hier kann nicht viel falsch gemacht werden, solange **Ubuntu 24.04 Desktop** als image gewählt wird. + +Für den PC im Roboter sollte die **Server** Variante des OS gewählt werden, da dieser ohne Bildschirm verwendet wird und dadurch Rechenleistung gespart werden kann, sowie die verwendung von **Remote-Tools** vereinfacht wird. + + + +  +  +  +  +# Ubuntu Grundlagen +Ein sehr vereinfachtes und unvollständiges Intro in die **Linux Shell**, das den Umgang mit **Befehlsterminals** lehren und erklären soll. +In Ubuntu lässt sich alles mit Terminalbefehlen erledigen. Gerade fürs Programmieren und den Umgang mit ROS ist es von enormer Bedeutung das Terminal nutzen zu können. + + +  +## Terminal UI +Die Tastenkombination `Strg`+`Alt`+`T` öffnet ein neues Terminal. Links in der aktuellen Zeile wird angezeigt, welcher **User** den Befehl ausführt, auf welchem **Host** der Befehl ausgeführt wird und in welchem **Verzeichnis** man sich gerade befindet, z.B.: + + resqbots@Q-T-Pi:~$ echo "hello world" + +`resqbots` ist der **User** + +`Q-T-Pi` ist der **Host** ( = das Gerät auf dem der User angemeldet ist) + +`~` ist das default Verzeichnis beim einloggen + +`$` ist ein Trennsymbol, danach fängt der Befehl an + +`echo "hello world"` ist der Befehl, der ausgeführt werden soll + + +  +## Wichtige Befehle + +### Strg + C +Die Tastenkombination `Strg` + `C` bricht den aktuell laufenden Prozess ab. Dies ist besonders wichtig, um ROS-Packages zu stoppen. + + + +  +### cd +`cd ` wechselt vom aktuellen Verzeichnis in das gewälte Unterverzeichnis. Mit `/` können mehrere Ebenen an Unterverzeichnissen aneinander gereiht werden. z.B.: + + cd ros_ws/src/resqbots_drive_interface/ + +Der Verzeichnisname `..` wechselt ein Verzeichnis nach "oben". So kommt man mit + + cd .. + +wieder in das Verzeichnis `ros_ws/src/` und mit + + cd ../.. +wäre man im Verzeichnis `ros_ws` + +Der Befehl `cd` ohne Verzeichnisnamen führt immer in das Standardverzeichnis zurück, egal wo man vorher war. + +Das `~` Symbol ist ein Platzhalter für den "standard" Arbeitsbereich. Wenn man also beispielsweise aus dem Verzeichnis `lidar_ws/src` in das Verzeichnis `moveit2_ws/src` wechseln will, gelingt das mit: + + cd ~/moveit2_ws/src + + + +  +### sudo +`sudo` steht kurz für "super user do" und entspricht dem **"Als Administrator ausführen"** bei Windows. Viele Befehle können nur mit dem Vorsatz `sudo` ausgeführt werden. Auch geschützte Dateien können nur mit `sudo` verändert werden. + + + +  +### apt install +`apt install ` wird quasi ausschließlich mit `sudo` gemeinsam verwendet und installiert das gewünschte Paket. + + sudo apt install hollywood + +installiert das hollywood Paket. (Ein netter Scherz, wenn man unwissende beeindrucken will.) + + + +  +### mkdir +`mkdir ` erstellt das gewünschte Verzeichnis. + + mkdir Test + +erstellt das Verzeichnis `Test` + +Der zusatz `-p` sorgt dafür, dass alle Übergeordneten Verzeichnisse unverändert bleiben. Mit `-p` können auch Unterverzeichnisse in einem Befehl mit-genereirt werden. + + mkdir -p ros_ws/src + +erstellt das Verzeichnis `src` im Verzeichnis `ros_ws`. Falls das Verzeichnis `ros_ws` nicht existiert, wird es ebenfalls erstellt. + + + +  +### rm +`rm ` wird eigentlich nur mit `sudo` davor verwendet. `sudo rm ` löscht die benannte Datei. +Mit dem zusatz `-r` können auch ganze Verzeichnisse gelöscht werden. + + sudo rm -r ros_ws + +löscht den oben erstellten `ros_ws` und **alle Unterverzeichnisse** + + + +  +### nano +`nano` ist eine Texteditor von Ubuntu. Damit können nahezu alle schreibbaren Dateiformate göffnet und bearbeitet werden. Für schreibgeschützte Dateien musst `sudo` vorgesetzt werden. +Mit `nano ` wird die benannte Datei geöffnet, oder, falls sie nicht existiert, erstellt. Mit `sudo nano` erstellte Dateien sind für "normale" User schreibgeschützt und können nur mit `sudo` bearbeitet werden. + + sudo nano README.txt + +erstellt z.B. eine schreibgeschützte `README.txt`. + + + +  +### Tab +Mit der `Tab` Taste können befehle automatisch vervollstöndigt werden. +Mit einem doppelten `Tab` werden alle verfügbaren Optionen angezeigt. + + + +  +### ssh +`ssh user@host` stellt eine Remoteverbindung zum gewählten Gerät her und loggt sich als der genannte User ein. Der **Host** kann dabei druch die **IP-Adresse** oder den **Gerätenamen** mit dem zusatz `.local` angegeben werden. + + ssh resqbots@192.168.1.42 + + ssh resqbots@q-t-pi.local + +loggt sich z.B. als user **resqbots** auf unserem RaspberryPi namens **Q-T-Pi** ein. + +Für das wiederholte Einloggen auf dem gleichen Gerät, sollte mit + + ssh-keygen + +ein **ssh public key** erstellt werden, der dann mit + + ssh-copy-id user@host + +auf das Zielgerät kopiert wird. Dadurch ist bei der Verbindung mittels `ssh` kein Passwort mehr nötig und es können z.B. skripte geschrieben werden, die durch die Remoteverbindung arbeiten. + + + +  +### pkill +`pkill` kann ausgeführte Programme und ROS Nodes beenden, wenn diese nicht auf `strg+c` reagieren. + + pkill --signal 2 -f /Pfad/zu/install/Verzeichnis + +`--signal 2` schickt ROS Nodes das standard Signal zum Runterfahren. Das ist der sanfteste Weg, eine Node zu beenden. (z.B. wenn das Fenster dazu geschlossen wurde, ohne die Node vorher zu schließen.) +`-f /Pfad` gibt den Pfad zum `/install` Ordner des Prozesses an, der geschlossen werden soll. + + + +  +### ./ + Dateiname +`./` führt das gewählte **Shell-Skript** aus, falls das möglich ist. In einem Skript können z.B. mehrere Befehle aneinander gereiht sein, oder Befehle mit vielen Optionen ausgeführt werden. +**Shell-Skripte** sind durch die Endung `.sh` gekennzeichnet und beginnen mit der Zeile `#!/bin/bash` + + + +  +## ROS2 Befehle + + +### ros2 run & ros2 launch +`ros2` ist ähnlich wie `sudo` eine Vorsilbe, die der Shell sagt, dass sie das Programm mit der ROS2 Umgebung ausführen soll. +`run` und `launch` sind zwei Varianten, Programme (Packages) in ROS zu starten. `run` startet das Programm in seiner einfachsten Form. `launch` startet das **launch file** des Packages und kann oft mit einer vielzahl von Optionen versehen werden. + + ros2 run joy joy_node + +startet z.B. das ROS2-Standardprogramm für Controller (X-Box/Playstation/etc.). `joy` ist der Name des Programms, `joy_node` ist die Node (also die Funktion), die ausgeführt werden soll. + + ros2 launch tele_op operator.launch.py + +startet unser `tele_op` Package und die `joy_node`. Im `operator.launch.py`-Launch-File steht, welche Nodes mit welchen Optionen gestartet werden sollen. +Die meisten launch Befehle enden auf `.launch` und `.py` oder `.cpp`. `.launch` signalisiert, dass es sich um eine Launch-Datei handelt, `.py` zeigt, dass es eine **Python**-Datei ist, `.cpp` steht für **C++**. + + + +  +### weitere ROS2 Befehle +`ros2 node list` -> Liste aktiver ROS Nodes +`ros2 topic list` -> Liste aktiver ROS Topics +`ros2 topic echo` -> Zeigt angegebenes Topic an +`ros2 service list` -> Liste verfügbarer ROS Services +`ros2 contol *` -> ale Befehle im zusammenhang mit ros2_control (Liste der Hardwareinterfaces/Controller, Status der Interfaces/Controller, etc) +`ros2 run rqt_graph rqt_graph` -> Zeigt eine graphische Darstellung aller aktiven Nodes, Services, Topics, etc +`sudo apt install ros-jazzy-PackageName` -> installiert das genannte Package + + + + +  +## Nützliche Programme +`network-manager` -> Netzwerk-Management +`net-tools` -> Netzwerkadapter +`iperf3` -> Netzwerkgeschwindigkeit +`nmap` -> Geräte im Netzwerk finden + + + + + +  +  +  +  +# ROS2 installieren +Wir verwenden auf unseren Rechnern `ROS2 Jazzy`. Für die Installation folgt am besten der [offiziellen ROS2 Jazzy Installationsanleitung](https://docs.ros.org/en/jazzy/Installation/Ubuntu-Install-Debs.html) + + +### ROS einrichten: +Alle Packages in Ros müssen **gesourced** werden, damit sie von der Shell gestartet werden können. +Um ein Package schnell in der aktuellen Shell zu sourcen kann der Befehl + + source $package_ws$/install/setup.bash + +verwendet werden. Wobei `$package_ws$` durch den Pfad zum Ordner, in dem der `colcon-build` Befehl ausgeführt wurde, ersetzt werden muss. (z.B. ~/ros_ws) +Falls man den `colcon-build` Befehl im aktuellen Ordner verwendet hat, reicht `source install/setup.bash`. + +Um ein Package permanent zu sourcen muss der Befehlt in die **~/.bashrc** Datei geschrieben werden. Das ist eine Setup Datei, die der Shell sagt, was sie vor dem Start alles machen muss. (z.B. ROS sourcen, oder die Hintergrundfarbe ändern.) + + echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc + +Die **DOMAIN_ID** sagt ROS welche Geräte in deinem Netzwerk zur gleichen Gruppe gehören. Sie ist per Default auf **0**, kann aber verändert werden. Wir haben sie aus Spaß und zur Sicherheit auf **17** gelegt. +Auch das muss in der Shell eingestellt werden: + + echo "export ROS_DOMAIN_ID=17" >> ~/.bashrc + +**rosdep** ist ein Verzeichnis in dem viele Packages über alle aktiven ROS Distros hinterlegt sind. Manchmal hilft es, das **rosdep** zur Verfügung zu haben. + + sudo rosdep init && rosdep update + +Zuletzt sollte ein **Workspace** erstellt werden, in dem an eigenen Packages gearbeitet werden kann: + + cd && mkdir -p ros_ws/src + +Grundsätzlich werden ROS Packages, die nicht einfach per `sudo apt install` installiert werden können in **Workspaces** gespeichert und gebuildet, damit das Dateimanagement vereinfacht wird. Die Workspaces sinnvoll zu benennen ist dabei von Vorteil. + + + + + +  +  +  +  +# ROS2 Packages installieren und einrichten +Packages sind Programme, die im ROS2 Framework ausgeführt werden können. Für die meisten Funktionen eines Roboters gibt es bereits fertige Packages. Bei einigen müssen nur geringere Einstellungen angepasst werden, um sie mit unserem Roboter zu verwenden, andere müssen stärker angepasst werden. Falls ein eigenes Package geschrieben werden muss, kann aber auch dabei auf Grundbausteine aus der ROS2 Bibliothek zurückgegriffen werden. + + + + + +### Package-Liste +    **Comunity Packages** +        [**Steuerung**](#steuerung) +        [joy](#joy) -> liest Controllerdaten aus +        [robot_state_publisher](#robot-state-publisher) -> zeigt Position und Lage des Roboters an +        [ros2_control und ros2_controller](#ros2_control-und-ros2_controller) -> Motoren ansteuern und Feedback einholen +        [moveit2](#moveit2) -> Armkinematik + +        [**Sensorik**](#sensorik) +        [audio_common](#audio_common) -> Micro und Lautsprecher +        [camera_ros]() -> USB Kamera Interface + +        [**LiDAR**](#lidar-livox-mid-360) +        [Livox_SDK2](#livox_sdk2)-> LiDAR Interface +        [livox_ros_driver2](#livox_ros_driver2) -> LiDAR Interface +        [PCL](#pcl) -> support für Fast_LIO2 +        [Eigen](#eigen3) -> support für Fast_LIO2 +        [Fast_LIO2](#fast_lio2) -> LiDAR Mapping +        [nav2_map_server](#nav2_map_server) -> speichert LiDAR Karten + +        [**Anzeige**](#anzeige) +        [rviz2]() -> LiDAR Karte und robot_state +        [rqt_image_view]() -> zeigt Kamerabilder an + +        **Support Packages** +        v4l2 +        image-transport-plugins +        ioport + +        [**Development-Tools**](#development-tools) +        [xacro]() +        [joint_state_publisher_gui]() +        []() + +    **Eigene Packages** +        [lotti2_teleop]() -> verteilt User Input an die ros2_controller +        [lotti2_control]() -> ros2_control Package für den "Lotti" Roboter. Enthält das Robotermodell, Infos über alle Motoren und die Interfaces um diese anzusteuern. +        [lotti2_flipper_controller]() -> Wandelt User Input in Motorbefehle um. + + + + + + +  +  +## Steuerung + +### joy +`joy` liest die Befehle eines standard Spielecontrollers aus und published sie im Topic `/joy`. +`joy` ist normalerweise bereits in der ROS2 Basisinstallation enthalten. Falls `joy` aus irgendeinem Grund nicht direkt nach der ROS2 Installation läuft, kann es, wie alle Basispackages, nachträglich installiert werden. + + sudo apt install ros-jazzy-joy + + + + +  +### robot state publisher +Der `robot_state_publisher` ist eine Node, die den aktuellen Status des Roboters published. Das ist notwendig für komplexere Bewegungsbefehle und zur visualisierung des Roboters. Der `robot_state_publisher`alleine tut nicht viel, ist aber ein notwendiger Bestandteil der ros2_control Arbeitskette. + + sudo apt install ros-jazzy-robot-state-publisher + + + + +  +### ros2_control und ros2_controller +Für die Steuerung des Roboters verwenden wir `ros2_control`. Das ist eine mächtige Controller-Interface Kombination, die extra für die Steuerung von Roboter entwickelt wurde. Für tiefere Einblicke in die Verwendung ist am besten eines der verlinkten Tutorials geeignet. + +`ros2_control` und `ros2_controller` sind standard Packages in ROS2 und können nur gemeinsam verwendet werden, daher sollte man immer beide installieren. + + sudo apt install ros-jazzy-ros2-control ros-jazzy-ros2-controllers + + + + +  +### Moveit2 +`Moveit2` enthält mächtige Tools für inverse Kinematiken. Wir nutzen dieses Package um unseren Arm zu steuern. +Auch, wenn nicht alle Komponenten von `Moveit2` benötigt werden, lohnt es sich alle zu installieren, damit sie zur Verfügung stehen. + + sudo apt install ros-jazzy-moveit* + + + + + +  +  +  +  +## Sensorik + +### audio_common +Dieses Setup ermöglicht die Audio-Kommunikation über ROS 2 Humble zwischen einem Laptop und einem Raspberry Pi – in beide Richtungen: +Laptop → Raspberry Pi (Audioaufnahme am Laptop, Wiedergabe am Pi) +Raspberry Pi → Laptop (Audioaufnahme am Pi, Wiedergabe am Laptop) + +**Installation: Audio-Tools und Dev-Packages** + + sudo apt install -y + libasound2-dev + gstreamer1.0-plugins-base + libgstreamer1.0-dev + liborc-0.4-dev + libdw-dev libelf-dev libunwind-dev + espeak alsa-utils pavucontrol + +**ROS2 Workspace einrichten - auf BEIDEN Geräten!** + + mkdir -p ~/ros2_audio_ws/src + cd ~/ros2_audio_ws/src + git clone -b ros2 https://github.com/ros-drivers/audio_common.git + +**Abhängigkeiten & Workspace bauen** + + cd ~/ros2_audio_ws + rosdep install --from-paths src --ignore-src -r -y + colcon build --symlink-install + +**Kommunikation Raspberry Pi → Laptop** + +**Raspberry Pi (Sender)** + + export AUDIODEV=plughw:1,0 + source ~/ros2_audio_ws/install/setup.bash + ros2 run audio_capture audio_capture_node --ros-args -r __node:=capture_pi + +**Laptop (Empfänger)** + + source ~/ros2_audio_ws/install/setup.bash + ros2 run audio_play audio_play_node --ros-args -r __node:=play_laptop + +**Verifikation & Tests** + +**Verbindung prüfen (z. B. auf dem Pi):** + + ros2 topic info /audio + +**Erwartete Ausgabe:** + + Type: audio_common_msgs/msg/AudioData + Publisher count: 1 + Subscription count: 1 + +**Live-Daten anzeigen:** + + ros2 topic echo /audio + +**Testausgabe senden (z. B. am Laptop):** + + espeak "Hallo Raspberry" --stdout | aplay + + + + +  +### camera_ros +Wir verwenden das `camera_ros` Package für unsere USB Kameras. +Da `camera_ros` ein Standardpackage von `humble` ist, gestaltet sich die Installation ganz einfach: + + sudo apt install ros-jazzy-camera-ros + +Die Einstellungen für die individuellen Kameras werden in `.yaml` Dateien festgehalten. + + + + + + +  +  +  +  +## LiDAR: Livox MID 360 +Wir arbeiten mit dem **Livox MID 360** LiDAR. Um diesen nutzen zu können müssen auf dem Gerät, das die Daten vom LiDAR auslesen soll die Packages `Livox_SDK2` und `livox_ros_driver2` installiert sein. +Um die Daten vom LiDAR zu einer Karte zu verarbeiten, verwenden wir `Fast_LIO2`. Das wiederum benötigt `Eigen3` und `PCL` um zu funktionieren. + + +Die Links zu den Originalprogrammen findet ihr hier: +**Livox_SDK2:** https://github.com/Livox-SDK/Livox-SDK +**Fast_LIO2:** https://github.com/hku-mars/FAST_LIO2 +**livox_ros_driver2:** https://github.com/Livox-SDK/livox_ros_driver2 + + +  +Vor der Installation sollte ein **Workspace** für die Livox Packages erstellt werden: + + cd && mkdir -p livox_ws/src + + + + +  +### Livox_SDK2 +Das GitHub Repo der **SDK** in den **Workspaca** kopieren + + cd ~/livox_ws/src + git clone https://github.com/Livox-SDK/Livox-SDK2.git + +Da die **SDK** ursprünglich für frühere ROS Versionen entwickelt wurde müssen vor dem builden noch Änderungen vorgenommen werden. Dazu wird die Zeile `#include ` in den Bereichen mit den anderen `#include` Zeilen in den Dateien `sdk_core/comm/define.h` und `sdk_core/logger_handler/file_manager.h` eingefügt: + + cd Livox-SDK2 + nano sdk_core/comm/define.h + nano sdk_core/logger_handler/file_manager.h + +Danach ist das Package bereit für den build-Prozess: + + mkdir build && cd build + cmake .. + make -j + +Nach die Umsetzung des Befehls `make -j` dauert extrem lang und beim RaspberryPi hängt sich wahrscheinlich die ssh Verbindung auf. Nicht wundernn, einfach das Terminal schließen und nach ca. 15 Minuten neu verbinden und weiter machen. + +**Fals Verbindung abgebrochen** + +ssh aufbauen, dann + + cd livox_ws/src/Livox-SDK2/build + +**weiter builden** + + sudo make install + +Auch der build-Prozess dauer ewig, nicht wundern. `Livox_SDK2` ist nur ein support Package für `livox_ros_driver2` und muss nicht gesourced werden. + + + + +  +### livox_ros_driver2 +Das GitHup Repo in den **Workspace** kopieren + + cd ~/livox_ws/src + git clone https://github.com/Livox-SDK/livox_ros_driver2.git + +und dann builden + + cd livox_ros_driver2 + ./build.sh humble + +Der Parameter `humble` funktioniert auch bei humble, das Package ist einfach etwas älter. + +Nach dem erfolgreichen Builden das Package sourcen: + + source ~/livox_ws/install/setup.bash + echo "source ~/livox_ws/install/setup.bash" >> ~/.bashrc + + + + +  +### PCL +PCL ist eine Support-Suite, die viele Mathematische Prozesse im Bezug auf Punktewolken übernimmt. +Sie ist einfach zu installieren: + + sudo apt update + sudo apt install pcl-* + sudo apt install libpc-dev libpcl-ros-dev + + + + + +  +### Eigen3 +Eigen ist eine Sammlung an Header-Files, die den build Prozess anderer Packages unterstützen kann. Sie wird benötigt um `Fast_LIO2` builden zu können. + +Für Eigen sollte ein extra **Workspace** erstellt werden, da es sonst zu ungewollten interaktionen mit anderen Packages kommen könnte. Der restliche Installationsprozess läuft wie immer ab. + + git clone https://gitlab.com/libeigen/eigen.git + cd eigen + mkdir build && cd build + cmake .. + make -j + sudo make install + +Auch hier ist ein **sourcen** nicht nötig. + + + + +  +### Fast_LIO2 +Zu guter Letzt kann endlich `Fast_LIO2` installiert werden. + +`Fast_LIO2` ist ein Package, das LiDAR Daten zu einer Karte zusammenfügen kann, ohne dafür **Odometry** Daten zu benötigen, indem es ähnlich dem Menschen "versteht" , dass sich Wände nicht bewegen, sonder der LiDAR selbst. (Nur mit deutlich mehr Mathe dahinter.) + +Auch für `Fast_LIO2` wird ein eigener **Workspace** erstellt, das das Package wieder lang zum builden braucht und nicht versehentlich neu gebuildet werden sollte, wenn man nur ein eigenes Package testen will. + + cd && mkdir -p fast_lio2_ws/src + cd fast_lio2_ws/src + git clone https://github.com/Ericsii/FAST_LIO2_ROS2.git --recursive + cd .. + rosdep install --from-paths src --ignore-src -y + +Nachdem das GitHub Repo in den **Workspace** kopiert und durch `rosdep install` erweitert wurde, muss noch die `CMakeLists.txt` Datei überarbeitet werden um mit der neuen ROS Version zu funktionieren. + + nano src/FAST_LIO2_ROS2/CMakeLists.txt + +Und an allen Stellen das **c++14** und **c++17** durch **c++20** ersetzen. + +Danach kann das Package mit `colcon` gebaut und im Anschluss gesourced werden. + + colcon build --symlink-install + source install/setup.bash + echo "source ~/fast_lio2_ws/install/setup.bash" >> ~/.bashrc + +Falls das Package auf einem Gerät installiert wurde, das selbst nicht live die erstellte Karte anzeigen soll muss noch die **launch-Datei** angepasst werden. + + nano install/fast_lio2/share/fast_lio2/launch/mapping.launch.py + +Die Zeile `ld.add_action(rviz_node)` mit **#** auskommentieren. + + + + +  +### Nav2_map_server +`Nav2` ist eine Package Suite für die Roboternavigation. Das Subsystem `nav2_map_server` verwenden wir, um die in Fast_LIO2 erstellten Karten zu speichern. + +**Installation:** + + sudo apt update + sudo apt install ros-jazzy-nav2-map-server + +Der Nav2_map_server wird beim Start von Fast_LIO2 automatisch mit gestartet. + + + + +  +  +### LiDAR Einrichten +Um den Lidar verwenden zu können müssen noch ein paar Schritte erledigt werden. + +Die `livox_ros_driver2` kann nur mit **statischen IP Adressen** arbeiten. Daher muss im Gerät, das die LiDAR Daten auslesen soll, **DHCP** ausgeschaltet werden. Mit + + cd /etc/netplan/ + ls + +werden alle Dateien im Verzeichnis `netplan` angezeigt. Normalerweise ist es nur eine, die `50-cloud-init.yaml` oder so ähnlich heißt. Diese `.yaml` Dateien sind schreibgeschützt, müssen also mit `sudo` geöffnet werden. + + sudo nano + +Das Terminal sollte nun ca. so aussehen: + + network: + wifis: + wlan0: + dhcp4: true + + +Unter dem Existierenden Abschnitt für `wifis` den folgenden Abschnitt einfügen: + + ethernets: + eth0: + dhcp4: false + addresses: + - 192.168.1.50/24 + optional: true + +Nach dem Speichern und Verlassen der Datei müssen die Änderungen übernommen werden: + + sudo netplan apply + +Jetzt hat das Gerät eine **statische IP** von 192.168.1.50 im **Ethernet**. Das Gerät kann jetzt nicht mehr über Ethernet angesteuert werden. Wird das benötigt, einfach das `dhcp4: false` durch `dhcp4: true` ersetzen. + + + +Jetzt muss in der **Konfig-Datei** die **IP Adresse** festgehalten werden. +Der Befehlt + + nano livox_ws/install/livox_ros_driver2/share/livox_ros_driver2/config/MID360_config.json + +zeigt die `MID360_config.json` Datei. Sie sieht nach der ersten Installation wie folgt aus: + +
+{
+"lidar_summary_info" : {
+	"lidar_type": 8
+},
+"MID360": {
+	"lidar_net_info" : {
+	"cmd_data_port": 56100,
+	"push_msg_port": 56200,
+	"point_data_port": 56300,
+	"imu_data_port": 56400,
+	"log_data_port": 56500
+	},
+	"host_net_info" : {
+	"cmd_data_ip" : "192.168.1.5",
+	"cmd_data_port": 56101,
+	"push_msg_ip": "192.168.1.5",
+	"push_msg_port": 56201,
+	"point_data_ip": "192.168.1.5",
+	"point_data_port": 56301,
+	"imu_data_ip" : "192.168.1.5",
+	"imu_data_port": 56401,
+	"log_data_ip" : "",
+	"log_data_port": 56501
+	}
+},
+"lidar_configs" : [
+	{
+	"ip" : "192.168.1.12",
+	"pcl_data_type" : 1,
+	"pattern_mode" : 0,
+	"extrinsic_parameter" : {
+		"roll": 0.0,
+		"pitch": 0.0,
+		"yaw": 0.0,
+		"x": 0,
+		"y": 0,
+		"z": 0
+	}
+	}
+]
+}
+
+ +Im Abschnitt `"MID360":` überall die **IP Adresse** `192.168.1.5` durch die `192.168.1.50` ersetzen. + +Im Abschnitt `"lidar_configs" :` kann die **IP Adresse** des LiDARS eingestellt werden. Um diese zu erfahren, den LiDAR mit einem Netzwerk verbinden, einen PC mit dem gleichen Netzwerk verbinden und + + sudo apt update && sudo apt install nmap + ip a #zeigt deine IP Adresse + nmap # nur die letzte Zahl ersetzen durch 0/24 + +In der Liste sollte irgendwo der LiDAR auftauchen. Die **Default IP** unseres LiDARs ist `192.168.1.196`. + + + + +  +### Lidar Optionen +In der Datei `*/src/FAST_LIO2_ROS2/config/mid360.yaml` können Einstellungen für den LiDAR Betrieb vorgenommen werden. `*` steht für die übergeordneten Verzeichnisse. In unserem Fall wahrscheinlich `~/fast_lio2_ws` + + nano ~/fast_lio2_ws/src/FAST_LIO2_ROS2/config/mid360.yaml + +Gibt folgendes aus: + +
+/**:
+	ros__parameters:
+		feature_extract_enable: false
+		point_filter_num: 3
+		max_iteration: 3
+		filter_size_surf: 0.5
+		filter_size_map: 0.5
+		cube_side_length: 1000.0
+		runtime_pos_log_enable: false
+		map_file_path: "./test.pcd"
+
+		common:
+			lid_topic:  "/livox/lidar"
+			imu_topic:  "/livox/imu"
+			time_sync_en: false         # ONLY turn on when external time synchronization is really >
+			time_offset_lidar_to_imu: 0.0 # Time offset between lidar and IMU calibrated by other al>
+										# This param will take effect no matter what time_sync_en is>
+
+		preprocess:
+			lidar_type: 1                # 1 for Livox serials LiDAR, 2 for Velodyne LiDAR, 3 for ou>
+			scan_line:  4
+			blind: 0.5
+			timestamp_unit: 3
+			scan_rate: 10
+
+		mapping:
+			acc_cov: 0.1
+			gyr_cov: 0.1
+			b_acc_cov: 0.0001
+			b_gyr_cov: 0.0001
+			fov_degree:    360.0
+			det_range:     100.0
+			extrinsic_est_en:  true      # true: enable the online estimation of IMU-LiDAR extrinsic
+			extrinsic_T: [ -0.011, -0.02329, 0.04412 ]
+			extrinsic_R: [ 1., 0., 0.,
+							0., 1., 0.,
+							0., 0., 1.]
+
+		publish:
+			path_en: true                # true: publish Path
+			effect_map_en: false         # true: publish Effects
+			map_en: true                 # true: publish Map cloud
+			scan_publish_en:  true       # false: close all the point cloud output
+			dense_publish_en: false      # false: low down the points number in a global-frame point>
+			scan_bodyframe_pub_en: true  # true: output the point cloud scans in IMU-body-frame
+
+		pcd_save:
+			pcd_save_en: true
+			interval: -1                 # how many LiDAR frames saved in each pcd file; 
+										# -1 : all frames will be saved in ONE pcd file, may lead to>
+
+ + +Bei `map_file_path:` den gewünschten Pfad + Dateinamen eingeben z.B. `"~/maps/latest_map.pcd"` + +`scan_rate:` kann auf 10, 30, 50 oder 100 gesetzt werden und steht für die Abtastrate des LiDAR. **Vorsicht:** je höher die `scan_rate` desto mehr Rechenleistung wird für die Karte benötigt und desto schneller füllt sich der Arbeitsspeicher. + + + + +  +  +### Lidar verwenden +**Lidar Starten** +Auf dem Laptop im Terminal im `/home directory` + + ./lidar_launch.sh + +eingeben, für automatischen Start. +Oder auf dem Mini PC 2 Terminals öffnen + + Terminal1: ros2 launch livox_ros_driver2 msg_MID360_launch.py + Terminal2: ros2 launch fast_lio2 mapping.launch.py config_file:=mid360.yaml + +**Karte speichern** + +Automatisch im Laptopterminal `/home directory` mit + + /.save_map.sh +es erscheint nun eine Meldung im Terminal: + + waiting for service to become available... + requester: making request: std_srvs.srv.Trigger_Request() + +und kurz darauf folgt: + + response: + + std_srvs.srv.Trigger_Response(success=True, message='Map saved.') +in diesem Fall war das speichern erfolgreich. + +Falls nach einigen Sekunden keine weitere Nachricht folgt, ist ein Fehler aufgetreten. +Die einfachsten möglichen Ursachen sind: +- Das Lidar Programm läuft nicht +- Die Verbindung zum MiniPC ist unterbrochen +- Der map_saver_server läuft nicht + + + + + +  +  +  +  +## Anzeige + +### rviz2 +Das Modell des Roboters und die aktuelle Lidar Karte visualisieren wir mit `rviz2`. + + sudo apt install ros-jazzy-rviz2 + + + + +  +### rqt_image_view +Für die Anzeige der Kamerabilder verwenden wir `rqt_image_view`. Es ist Teil der `rqt`-Suite, die für viele Funktionen in ROS2 hilfreich ist. Sie sollte bei der ROS Basisinstallation bereits heruntergeladen sein. Falls nicht kann entweder nur `rqt_image_view` oder die gesamte rqt-Suite installiert werden: + + sudo apt install ros-jazzy-rqt-image-view + + sudo apt install-ros-jazzy-rqt-* + + + + + +  +  +  +  +## Development-Tools + +### xacro +`xacro` ist eine Erweiterung, die es erlaubt, eine `.URDF` auf mehrere Dateien aufzuteilen. Für mehr Infos siehe Abschnitt [URDF](#urdf). `xacro` ist kein ROS2 Package und muss deshalb installiert werden. + + sudo apt install ros-jazzy-xacro + + + + +  +### joint_state_publisher_gui +Das `joint_state_publisher_gui` startet ein graphisches Interface mit Schiebereglern für alle momentan gepublishten Joints. Durch verschieben der Regler wird der Zustand des Joints angepasst. Dieses Tool hilft beim testen einer neuen oder angepassten `URDF` und hilft bei der Fehlersuche, falls ein Robotermodell nicht wie gedacht funktioniert. + + sudo apt install ros-jazzy-joint-state-publisher-gui + + + + + + + + + +  +  +  +  +# Packages entwickeln + +### Grundlagen +Um von Null an zu lernen, wie man ein ROS2 Package entwickelt sollten die [Tutorials](https://docs.ros.org/en/humble/Tutorials.html) der ROS2 Dokumentation abgearbeitet werden. Danach sind die Basics klar(er) und es kann mit der Entwicklung komplexerer Programme fortgefahren werden. + + + + +  +## ros2_control Packages + + + +Für ros2_control Packages ist eine gewisse Ordnerstruktur notwendig, damit das System alle Komponenten findet. Der folgende Block zeigt eine Beispielstruktur, in der jede Enrückung eine niedrigere Ordnerebene darstellt. Bezeichnungen ohne Dateiendung sind Ordner. Die Funktionen und der Inhalt der jeweiligen Ordner werden im Verlauf des Kapitels genauer erklärt. Die wichtigsten Segmente sind mit Links versehen. + + +Package_name +        bringup +                config +                    [controllers.yaml](#controllers.yaml) +        [launch](#launch-file-erstellen) +            full.launch.py +            teil.launch.py +            ... +        description +                meshes +                        link1.stl +                        link2.stl +                        link3.stl +                        ... +                [ros2_control](#ros2_controlxacro) +                        ros2_control.xacro +                [srdf](#srdf) +                        srdf_arm.xacro +                        srdf_body.xacro +                        ... +                [urdf](#urdf) +                        Robot.urdf.xacro +                        arm.xacro +                        body.xacro +                        sensors.xacro +                        ... +                [hardware](#hardware-interfaces) +                        include +                                Package_name +                                        interface1.hpp +                                        interface2.hpp +                                        utility.hpp +                        interface1.cpp +                        interface2.cpp +                CMakeLists.txt +                package.xml +                [ros2_control.xml](#ros2_controlxml) + + + + + + + +### URDF +Das `.urdf` File enthält Informationen über alle Bauteile und Gelenke des Roboters. Ein `.urdf` File kann als grobe Abstraction manuell erstellt, oder aus einem CAD Modell exportiert werden. +Um die Grundlagen von URDF zu verstehen, ist es ratsam, zunächst manuell ein einfachens `.urdf` File zu schreiben. Dabei helfen diese Tutorials: + +[ROS2 URDF Tutorial](https://docs.ros.org/en/humble/Tutorials/Intermediate/URDF/URDF-Main.html) +[Articulated Robotics Tutorial mit Videos](https://articulatedrobotics.xyz/tutorials/ready-for-ros/urdf/) + +Ein `.urdf`File kann auch ohne eine fertige Steuerung inspiziert werden. +Falls das `.urdf` File mit `xacro` erstellt wurde, muss es erst mit + + xacro file_name.urdf.xacro > file_name.urdf + +zu einem normalen `.urdf` File kompiliert werden. Danach 2 Terminals öffnen. + + 1. ros2 run robot_state_publisher robot_state_publisher file_name.urdf + 2. ros2 run joint_state_publisher_gui joint_state_publisher_gui + +`1.` publisht die Position und Lage des Robots, `2.` published das `/tf` Topic und ermöglicht, die Gelenke des Roboters durch Schieber zu bewegen. + + + + +  +**URDF aus CAD** + +Um ein `.urdf` File **aus einem CAD Modell zu extrahieren** z.B. für SolidWorks [hier](https://github.com/ros/solidworks_urdf_exporter) das Plugin installieren und [diesem](https://wiki.ros.org/sw_urdf_exporter/Tutorials/Export%20an%20Assembly) Tutorial folgen. Nach dem Export die URDF mit einem online URDF-Viewer-Tool überprüfen (z.B. [diesem](https://viewer.robotsfan.com/) hier). Dabei auf sinnvolle Rotationsachsen und -limits achten. + +Wenn die exportierten Dateien nach der Überprüfung zufriedenstellend sind, die Ordner `meshes` und `urdf` aus der exportierten Datei in den Ordner `description` kopieren. Anschließend in der `.urdf` Datei bei allen `links` den Dateipfad für die meshes anpassen. + + + +Hierbei hilft es, den Abschnitt `"package://PACKAGENAME` zu markieren und alle Instanzen davon zu bearbeiten (in VS Code standardmäßig mit `strg`+`F2`). + +Bei Robotern mit vielen unabhängig beweglichen Teilen ist es sinnvoll, die URDF nach Bewegungsgruppen aufzuteilen, um die Übersicht zu behalten. Dann muss auch nicht immer die ganze URDF neu exportiert werden, wenn in der CAD Konstruktion eine Baugruppe überarbeitet wird. + + + + +### SRDF + + + + + +### ros2_control.xacro + +In dieser Datei werden alle ROS2 Hardware Interfaces aufgezählt, welche Gelenke ihnen zugeordnet sind und welche state- und command-interfaces sie claimen. +Zusätzlich können hier Parameter hinterlegt werden, die von den Interfaces beim Start ausgelesen werden. Nach Änderungen in dieser Datei muss nicht neu gebuildet werden. + +**Beispieldatei:** + + + + + + + + robot_interface1/RobotInterface1 + value1 + value2 + + + + + + + + + + + + + + + + + + + + + + + + robot_interface2/RobotInterface2 + value3 + value4 + + + + + + + + + + + + + + + + + + + + + + +Der Header + + + + +ist notwendig um `xacro` zu verwenden. + +`` zeigt dem Programm an, dass hier die Hardware Interfaces aufgelistet werden. +Das Argumente `name="GroupName"` beschreibt, welchen Teil des Roboters die Hardware Interfaces steuern, ist aber nur inerhalb dieser Datei relevant. Falls es z.B. ein Interface für die Räder und eines für den Arm gibt, braucht man zwei ros2_control Gruppen. +`type="system"` zeigt an, dass es sich um ein System aus Roboterteilen handelt. Andere Kategorien sind für uns vorerst irrelevant. +`rw_rate="50"` setzt die **read-write-Rate** auf 50 Hz. + +Mit dem Wraper `` wird die Beschreibung des Hardware Interfaces für diese Gruppe gestartet. `` beschreibt, welches Hardware interface verwendet werden soll. Der name muss mit dem in der `ros2_control.xml` Datei übereinstimmen. +`` bestimmt die Namen der Parameter, an Stelle von `value1` kommt der Wert des Parameters. Dies kann eine Zahl oder Text sein. + +Nach dem `` Abschnitt werden die Gelenke des Roboters aufgelistet, die das Interface steuern kann. Jedes Gelenk startet mit `` der Name des Gelenks muss mit dem in der URDF übereinstimmen. Nach dem Namen des Gelenks wird aufgezählt, welche **command_interfaces** und welche **state_interfaces** das Gelenk hat. **command_interfaces** sind die Befehle, die an den Motor geschickt werden können. **state_interfaces** sind Informationen, die der Motor zurück schickt. + +**Alle Abschnitte die mit `` begonnen wurden, müssen am ende mit `` geschlossen werden.** + + +  + +### Hardware Interfaces + +Im Ordner `hardware` werden alle Hardware-Interfaces angelegt. Diese sind für die Kommunikation zwischen den Controllern und den Motoren zuständig. Hier werden Umrechnungsfaktoren für Getriebe, Serielle Verbindungen und ähnliches hinterlegt. Generell muss für jede Bewegungsgruppe ein eigenes Hardware-Interface verwendet werden, selbst, wenn beide Gruppen die gleichen Motoren und Controller verwenden, sollte diese Regel beibehalten werden, um interne Konflikte zu vermeiden. Die Lotti2 Interfaces sind so umfangreich, wie möglich kommentiert, um als Vorlage dienen zu können. +Im Ordner `include` werden die Header-Files der Interfaces angelegt. Sie benennen die Art des Interfaces und etablieren Variablen, die durch die ganze Klasse verwendung finden. + + +  + +### ros2_control.xml + +Hier werden alle in diesem Package erstellten Hardware Interfaces aufgelistet, damit der ROS2 Controller Manager erkennt, dass er sie exportieren und damit "zur Verfügung stellen" muss. + +**Beispiel** + + + + + + + Interface to drive Lotti's flippers + + + + + + Interface to drive Lotti's tracks + + + + + + +  + +### controllers.yaml + +Die controllers.yaml speichert welche Controller im Roboter zum Einsatz kommen und deren Einstellungen und Parameter. Die Grundstruktur sieht immer gleich aus: + + controller_manager: + ros__parameters: + update_rate: 50 #Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + Bewegungsgruppe1_controller: + type: joint_trajectory_controller/JointTrajectoryController + + Bewegungsgruppe2_controller: + type: lotti2_flipper_controller/FlipperController + + + Bewegungsgruppe1_controller: + ros__parameters: + joints: + - joint_name_1 + - joint_name_2 + - joint_name_3 + command_interfaces: + - position + - effort + state_interfaces: + - position + - effort + - velocity + - temp + + parameter1: 50 + parameter2: 2.0 + + + Bewegungsgruppe2_controller: + ros__parameters: + joints: + - joint_name_1 + - joint_name_2 + command_interfaces: + - velocity + state_interfaces: + - position + - effort + - velocity + - temp + + parameter1: 50 + parameter2: 2.0 + + +Der Abschnitt + + controller_manager: + ros__parameters: + update_rate: 50 #Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +ist in jedem controller.yaml file gleich. Die default-update-Rate für alle Controller wird benannt und der joint_state_broadcaster wird iniziiert. + +Danach folgen alle Controller, die beim Start des Roboters geladen werden sollen. Sie werden beim Namen genannt (Dieser dient allein der eindeutigen Identifizierung) und ihr Typ wird definiert. Der Typ muss mit einem existierenden Controllertyp übereinstimmen. Auch selbstgeschriebene Controller können hier genannt werden. (siehe z.B. lotti2_flipper_controller) + +Danach werden unter den Controllernamen die ihnen zugeordneten Gelenke (Joints) sowie die Command- und State-Interfaces und ihre Parameter aufgelistet. + +Die Formatierung dieser Datei ist entscheidend und muss der Vorlage entsprechen. Beim Builden werden Formatierungsfehler und falsche Bezeichnungen in der Regel nicht besonders gut benannt. Auch beim Start des Packages wird oft nur der Hinweis 'Error in controlers.yaml' oder etwas vergleichbares eingeblendet und der Prozess schlägt fehl. + +  + +### launch-File erstellen + + + + +  + +### CMakeLists.txt anpassen + + + + + + + + +  +  +  +  +## Moveit2 Integration + +## camera +### in order to launch all cameras run the launch file in lotti_vision folder : + ros2 launch lotti_vision multi_camera_launch.py +### to publish only of camera : + ros2 run webcam_publisher ffmpeg_camera_node --ros-args -p device: -p camera_name:= +#### example: + ros2 run lotti_vision ffmpeg_camera_node --ros-args -p device:=/dev/video0 -p camera_name:=camera_1 +### if you need to find the right port of the camera: + v4l2-ctl --list-devices +### run the subscriber manually by: + ros2 run lotti_vision camera_dashboard +### you can also check the frame rate by: + ros2 topic hz + ros2 topic hz /camera_1/image_raw/compressed + + + mock_components/GenericSystem + + + + + ${initial_positions['arm_link1_joint']} + + + + + + ${initial_positions['arm_link2_joint']} + + + + + + ${initial_positions['arm_link3_joint']} + + + + + + ${initial_positions['arm_link4_joint']} + + + + + + ${initial_positions['arm_link5_joint']} + + + + + + diff --git a/control_ws/src/lotti3_moveit_config/config/Lotti3.srdf b/control_ws/src/lotti3_moveit_config/config/Lotti3.srdf new file mode 100644 index 0000000..7f10024 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/Lotti3.srdf @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control_ws/src/lotti3_moveit_config/config/Lotti3.urdf.xacro b/control_ws/src/lotti3_moveit_config/config/Lotti3.urdf.xacro new file mode 100644 index 0000000..30cb1c6 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/Lotti3.urdf.xacro @@ -0,0 +1,14 @@ + + + + + + + + + + + + + + diff --git a/control_ws/src/lotti3_moveit_config/config/initial_positions.yaml b/control_ws/src/lotti3_moveit_config/config/initial_positions.yaml new file mode 100644 index 0000000..d6801d4 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/initial_positions.yaml @@ -0,0 +1,8 @@ +# Default initial positions for Lotti3's ros2_control fake system + +initial_positions: + arm_link1_joint: 0 + arm_link2_joint: 0 + arm_link3_joint: 0 + arm_link4_joint: 0 + arm_link5_joint: 0 \ No newline at end of file diff --git a/control_ws/src/lotti3_moveit_config/config/joint_limits.yaml b/control_ws/src/lotti3_moveit_config/config/joint_limits.yaml new file mode 100644 index 0000000..1e0414f --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/joint_limits.yaml @@ -0,0 +1,35 @@ +# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed + +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +default_acceleration_scaling_factor: 0.1 + +# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] +# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] +joint_limits: + arm_link1_joint: + has_velocity_limits: true + max_velocity: 0.25 + has_acceleration_limits: false + max_acceleration: 0 + arm_link2_joint: + has_velocity_limits: true + max_velocity: 2 + has_acceleration_limits: false + max_acceleration: 0 + arm_link3_joint: + has_velocity_limits: true + max_velocity: 3 + has_acceleration_limits: false + max_acceleration: 0 + arm_link4_joint: + has_velocity_limits: true + max_velocity: 1.1000000000000001 + has_acceleration_limits: false + max_acceleration: 0 + arm_link5_joint: + has_velocity_limits: true + max_velocity: 1.1000000000000001 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/control_ws/src/lotti3_moveit_config/config/kinematics.yaml b/control_ws/src/lotti3_moveit_config/config/kinematics.yaml new file mode 100644 index 0000000..dcf253e --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/kinematics.yaml @@ -0,0 +1,4 @@ +lotti_arm_group: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.0050000000000000001 + kinematics_solver_timeout: 0.050000000000000003 \ No newline at end of file diff --git a/control_ws/src/lotti3_moveit_config/config/moveit.rviz b/control_ws/src/lotti3_moveit_config/config/moveit.rviz new file mode 100644 index 0000000..deb3adb --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/moveit.rviz @@ -0,0 +1,51 @@ +Panels: + - Class: rviz_common/Displays + Name: Displays + Property Tree Widget: + Expanded: + - /MotionPlanning1 + - Class: rviz_common/Help + Name: Help + - Class: rviz_common/Views + Name: Views +Visualization Manager: + Displays: + - Class: rviz_default_plugins/Grid + Name: Grid + Value: true + - Class: moveit_rviz_plugin/MotionPlanning + Name: MotionPlanning + Planned Path: + Loop Animation: true + State Display Time: 0.05 s + Trajectory Topic: display_planned_path + Planning Scene Topic: monitored_planning_scene + Robot Description: robot_description + Scene Geometry: + Scene Alpha: 1 + Scene Robot: + Robot Alpha: 0.5 + Value: true + Global Options: + Fixed Frame: body_link + Tools: + - Class: rviz_default_plugins/Interact + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.0 + Focal Point: + X: -0.1 + Y: 0.25 + Z: 0.30 + Name: Current View + Pitch: 0.5 + Target Frame: body_link + Yaw: -0.623 +Window Geometry: + Height: 975 + QMainWindow State: 000000ff00000000fd0000000100000000000002b400000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d00000123000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000166000001910000018800fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a0056006900650077007301000002fd000000b5000000a400ffffff000001f60000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Width: 1200 diff --git a/control_ws/src/lotti3_moveit_config/config/moveit_controllers.yaml b/control_ws/src/lotti3_moveit_config/config/moveit_controllers.yaml new file mode 100644 index 0000000..8d0cff0 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/moveit_controllers.yaml @@ -0,0 +1,18 @@ +# MoveIt uses this configuration for controller management + +moveit_controller_manager: moveit_simple_controller_manager/MoveItSimpleControllerManager + +moveit_simple_controller_manager: + controller_names: + - arm_controller + + arm_controller: + type: FollowJointTrajectory + joints: + - arm_link1_joint + - arm_link2_joint + - arm_link3_joint + - arm_link4_joint + - arm_link5_joint + action_ns: follow_joint_trajectory + default: true \ No newline at end of file diff --git a/control_ws/src/lotti3_moveit_config/config/pilz_cartesian_limits.yaml b/control_ws/src/lotti3_moveit_config/config/pilz_cartesian_limits.yaml new file mode 100644 index 0000000..b2997ca --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/pilz_cartesian_limits.yaml @@ -0,0 +1,6 @@ +# Limits for the Pilz planner +cartesian_limits: + max_trans_vel: 1.0 + max_trans_acc: 2.25 + max_trans_dec: -5.0 + max_rot_vel: 1.57 diff --git a/control_ws/src/lotti3_moveit_config/config/ros2_controllers.yaml b/control_ws/src/lotti3_moveit_config/config/ros2_controllers.yaml new file mode 100644 index 0000000..8ffcb35 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/ros2_controllers.yaml @@ -0,0 +1,24 @@ +# This config file is used by ros2_control +controller_manager: + ros__parameters: + update_rate: 100 # Hz + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + +arm_controller: + ros__parameters: + joints: + - arm_link1_joint + - arm_link2_joint + - arm_link3_joint + - arm_link4_joint + - arm_link5_joint + command_interfaces: + [] + state_interfaces: + [] \ No newline at end of file diff --git a/control_ws/src/lotti3_moveit_config/config/sensors_3d.yaml b/control_ws/src/lotti3_moveit_config/config/sensors_3d.yaml new file mode 100644 index 0000000..99dcaeb --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,25 @@ +sensors: + - default_sensor + - kinect_depthimage +default_sensor: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 +kinect_depthimage: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /head_mount_kinect/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 \ No newline at end of file diff --git a/control_ws/src/lotti3_moveit_config/launch/demo.launch.py b/control_ws/src/lotti3_moveit_config/launch/demo.launch.py new file mode 100644 index 0000000..063f79c --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/demo.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_demo_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_demo_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/launch/move_group.launch.py b/control_ws/src/lotti3_moveit_config/launch/move_group.launch.py new file mode 100644 index 0000000..c6382e7 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/move_group.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_move_group_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_move_group_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/launch/moveit_rviz.launch.py b/control_ws/src/lotti3_moveit_config/launch/moveit_rviz.launch.py new file mode 100644 index 0000000..73d23d3 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/moveit_rviz.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_moveit_rviz_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_moveit_rviz_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/launch/rsp.launch.py b/control_ws/src/lotti3_moveit_config/launch/rsp.launch.py new file mode 100644 index 0000000..e532bee --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/rsp.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_rsp_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_rsp_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/launch/setup_assistant.launch.py b/control_ws/src/lotti3_moveit_config/launch/setup_assistant.launch.py new file mode 100644 index 0000000..77b1e78 --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/setup_assistant.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_setup_assistant_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_setup_assistant_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/launch/spawn_controllers.launch.py b/control_ws/src/lotti3_moveit_config/launch/spawn_controllers.launch.py new file mode 100644 index 0000000..e5513bf --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/spawn_controllers.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_spawn_controllers_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_spawn_controllers_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/launch/static_virtual_joint_tfs.launch.py b/control_ws/src/lotti3_moveit_config/launch/static_virtual_joint_tfs.launch.py new file mode 100644 index 0000000..d1a268d --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/static_virtual_joint_tfs.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_static_virtual_joint_tfs_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/launch/warehouse_db.launch.py b/control_ws/src/lotti3_moveit_config/launch/warehouse_db.launch.py new file mode 100644 index 0000000..56fbd9d --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/launch/warehouse_db.launch.py @@ -0,0 +1,7 @@ +from moveit_configs_utils import MoveItConfigsBuilder +from moveit_configs_utils.launches import generate_warehouse_db_launch + + +def generate_launch_description(): + moveit_config = MoveItConfigsBuilder("Lotti3", package_name="lotti3_moveit_config").to_moveit_configs() + return generate_warehouse_db_launch(moveit_config) diff --git a/control_ws/src/lotti3_moveit_config/package.xml b/control_ws/src/lotti3_moveit_config/package.xml new file mode 100644 index 0000000..2b9a09c --- /dev/null +++ b/control_ws/src/lotti3_moveit_config/package.xml @@ -0,0 +1,51 @@ + + + + lotti3_moveit_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the Lotti3 with the MoveIt Motion Planning Framework + + Res.Q Bots + + BSD + + http://moveit.ros.org/ + https://github.com/ros-planning/moveit2/issues + https://github.com/ros-planning/moveit2 + + Res.Q Bots + + ament_cmake + + moveit_ros_move_group + moveit_kinematics + moveit_planners + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + tf2_ros + xacro + + + + controller_manager + lotti_control3 + moveit_configs_utils + moveit_ros_move_group + moveit_ros_visualization + moveit_ros_warehouse + moveit_setup_assistant + robot_state_publisher + rviz2 + rviz_common + rviz_default_plugins + tf2_ros + warehouse_ros_mongo + + + + ament_cmake + + diff --git a/control_ws/src/lotti_control3/CMakeLists.txt b/control_ws/src/lotti_control3/CMakeLists.txt new file mode 100644 index 0000000..9e499ee --- /dev/null +++ b/control_ws/src/lotti_control3/CMakeLists.txt @@ -0,0 +1,150 @@ +cmake_minimum_required(VERSION 3.16) +project(lotti_control3 LANGUAGES CXX) + +option( + LOTTI_ENABLE_VENDOR_UNITREE_SDK + "Link the optional vendor Unitree SDK for the drive interface transport." + OFF +) + +set( + UNITREE_SDK_DIR + "" + CACHE PATH + "Optional directory containing libUnitreeMotorSDK_Linux64.so for the real drive transport." +) + +# Compiler options for GCC/Clang +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wall -Wextra) +endif() + +# Ensure Windows export all symbols (for DLLs) +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# Hardware interface dependencies +set(HW_IF_INCLUDE_DEPENDS + pluginlib + rcpputils + hardware_interface + rclcpp + rclcpp_lifecycle +) + +# Find dependencies +find_package(ament_cmake REQUIRED) + +foreach(Dependency IN ITEMS ${HW_IF_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +find_library(LIBSERIAL_LIBRARY NAMES serial libserial REQUIRED) + +# Your main shared library +add_library( + lotti_control3 + SHARED + hardware/arm3_interface.cpp + hardware/flipper3_interface.cpp + hardware/drive3_interface.cpp +) + +# Compile features and includes +target_compile_features(lotti_control3 PUBLIC cxx_std_17) +target_include_directories(lotti_control3 PUBLIC + $ + $ + $ +) + +ament_target_dependencies( + lotti_control3 PUBLIC + ${HW_IF_INCLUDE_DEPENDS} +) + +target_link_libraries(lotti_control3 PUBLIC ${LIBSERIAL_LIBRARY}) + +if(LOTTI_ENABLE_VENDOR_UNITREE_SDK) + set(UNITREE_SDK_SEARCH_PATHS) + + if(UNITREE_SDK_DIR) + list(APPEND UNITREE_SDK_SEARCH_PATHS + "${UNITREE_SDK_DIR}" + "${UNITREE_SDK_DIR}/lib" + ) + endif() + + list(APPEND UNITREE_SDK_SEARCH_PATHS "${CMAKE_CURRENT_SOURCE_DIR}/lib") + + find_library( + UNITREE_SDK_LIB + NAMES UnitreeMotorSDK_Linux64 libUnitreeMotorSDK_Linux64 + PATHS ${UNITREE_SDK_SEARCH_PATHS} + ) + + if(NOT UNITREE_SDK_LIB) + message(FATAL_ERROR + "LOTTI_ENABLE_VENDOR_UNITREE_SDK is ON, but libUnitreeMotorSDK_Linux64.so was not found. " + "Set UNITREE_SDK_DIR to the vendor SDK location, place it in control_ws/src/lotti_control3/lib/, or disable the option." + ) + endif() + + target_link_libraries(lotti_control3 PUBLIC ${UNITREE_SDK_LIB}) + target_compile_definitions(lotti_control3 PUBLIC LOTTI_HAVE_UNITREE_SDK=1) + message(STATUS "Building lotti_control3 with vendor Unitree SDK support enabled.") +else() + target_compile_definitions(lotti_control3 PUBLIC LOTTI_HAVE_UNITREE_SDK=0) + message(STATUS "Building lotti_control3 without the vendor Unitree SDK. The drive interface stays in stub mode.") +endif() + +# Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface lotti_control3.xml) + +# Install rules +install( + DIRECTORY hardware/include/ + DESTINATION include +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + DIRECTORY description/launch description/urdf description/srdf description/meshes description/worlds + DESTINATION share/lotti_control3/description +) + +install( + DIRECTORY description/ros2_control + DESTINATION share/lotti_control3 +) + +install( + DIRECTORY bringup/launch bringup/config + DESTINATION share/lotti_control3 +) + +install( + FILES lotti_control3.xml + DESTINATION share/lotti_control3 +) + +install(TARGETS lotti_control3 + EXPORT export_lotti_control3 + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Testing +if(BUILD_TESTING) + message(STATUS "No package-local tests are currently registered for lotti_control3.") +endif() + +# Export targets and dependencies +ament_export_targets(export_lotti_control3 HAS_LIBRARY_TARGET) +ament_export_dependencies(${HW_IF_INCLUDE_DEPENDS}) + +ament_package() diff --git a/control_ws/src/lotti_control3/bringup/config/lotti_controllers.yaml b/control_ws/src/lotti_control3/bringup/config/lotti_controllers.yaml new file mode 100644 index 0000000..70557da --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/config/lotti_controllers.yaml @@ -0,0 +1,58 @@ +controller_manager: + ros__parameters: + update_rate: 100 #Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + arm_controller: + type: joint_trajectory_controller/JointTrajectoryController + + flipper3_controller: + type: flipper3_controller/FlipperController + + drive_controller: + type: drive_controller/DriveController + +arm_controller: + ros__parameters: + joints: + - arm_link1_joint + - arm_link2_joint + - arm_link3_joint + - arm_link4_joint + - arm_link5_joint + command_interfaces: + - position + state_interfaces: + - position + allow_partial_joints_goal: true + open_loop_control: true + interpolate_from_desired_state: true + set_last_command_interface_value_as_state_on_activation: false + allow_nonzero_velocity_at_trajectory_end: true + constraints: + stopped_velocity_tolerance: 0.01 + goal_time: 0.05 + +flipper3_controller: + ros__parameters: + joints: + - flipper_fr_joint + - flipper_fl_joint + - flipper_rr_joint + - flipper_rl_joint + command_interfaces: + - velocity + state_interfaces: + - position + +drive_controller: + ros__parameters: + joints: + - left_chain_joint + - right_chain_joint + command_interfaces: + - velocity + state_interfaces: + - velocity \ No newline at end of file diff --git a/control_ws/src/lotti_control3/bringup/config/lotti_servo_config.yaml b/control_ws/src/lotti_control3/bringup/config/lotti_servo_config.yaml new file mode 100644 index 0000000..2872c38 --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/config/lotti_servo_config.yaml @@ -0,0 +1,71 @@ +############################################### +# Modify all parameters related to servoing here +############################################### +use_gazebo: false # Whether the robot is started in a Gazebo simulation environment + +## Properties of incoming commands +command_in_type: "unitless" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s +scale: + # Scale parameters are only used if command_in_type=="unitless" + linear: 1.0 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands. + rotational: 1.0 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands. + # Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic. + joint: 8.0 + +# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling) +override_velocity_scaling_factor: 0.2 # valid range [0.0:1.0] + +## Properties of outgoing commands +publish_period: 0.05 # 1/Nominal publish rate [seconds] +low_latency_mode: false # Set this to true to publish as soon as an incoming Twist command is received (publish_period is ignored) + +# What type of topic does your robot driver expect? +# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory +command_out_type: trajectory_msgs/JointTrajectory + +# What to publish? Can save some bandwidth as most robots only require positions or velocities +publish_joint_positions: true +publish_joint_velocities: false +publish_joint_accelerations: false + +## Plugins for smoothing outgoing commands +smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" + +# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, +# which other nodes can use as a source for information about the planning environment. +# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node), +# then is_primary_planning_scene_monitor needs to be set to false. +is_primary_planning_scene_monitor: true + +## MoveIt properties +move_group_name: lotti_arm_group # Often 'manipulator' or 'arm' +planning_frame: body_link # The MoveIt planning frame. Often 'base_link' or 'world' + +## Other frames +ee_frame_name: arm_link5_link # The name of the end effector link, used to return the EE pose +robot_link_command_frame: arm_link5_link # commands must be given in the frame of a robot link. Usually either the base or end effector + +## Stopping behaviour +incoming_command_timeout: 1.0 # Stop servoing if X seconds elapse without a new command +# If 0, republish commands forever even if the robot is stationary. Otherwise, specify num. to publish. +# Important because ROS may drop some messages and we need the robot to halt reliably. +num_outgoing_halt_msgs_to_publish: 4 + +## Configure handling of singularities and joint limits +lower_singularity_threshold: 90.0 # Start decelerating when the condition number hits this (close to singularity) +hard_stop_singularity_threshold: 100.0 # Stop when the condition number hits this +joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) + +## Topic names +cartesian_command_in_topic: cmd/arm/joy_twiststamped # Topic for incoming Cartesian twist commands +joint_command_in_topic: cmd/arm/joy_joint # Topic for incoming joint angle commands +joint_topic: /joint_states +status_topic: ~/status # Publish status to this topic +command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here + +## Collision checking for the entire robot body +check_collisions: false # Check collisions? +collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often. +self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m] +scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m] diff --git a/control_ws/src/lotti_control3/bringup/config/view_lotti.rviz b/control_ws/src/lotti_control3/bringup/config/view_lotti.rviz new file mode 100644 index 0000000..af01138 --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/config/view_lotti.rviz @@ -0,0 +1,278 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + - /RobotModel1 + Splitter Ratio: 0.5 + Tree Height: 853 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 + - Class: rviz_visual_tools/RvizVisualToolsGui + Name: RvizVisualToolsGui +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + arm_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_link1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_link2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_link3_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_link4_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + arm_link5_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + body_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flipper_fl_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flipper_fr_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flipper_rl_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + flipper_rr_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_main_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_main_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 168 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /Laser_map + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: true + Global Options: + Background Color: 119; 118; 123 + Fixed Frame: body_link + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 1.4820582866668701 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.015883082523941994 + Y: 0.04515325650572777 + Z: 0.25667673349380493 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.11979752033948898 + Target Frame: body_link + Value: Orbit (rviz_default_plugins) + Yaw: 1.4986108541488647 + Saved: ~ +Window Geometry: + Displays: + collapsed: true + Height: 1043 + Hide Left Dock: true + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd000000040000000000000164000003e0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730000000016000003e0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069000000039a000000410000004100ffffff000000010000010f000002effc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002ef000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073a0000003efc0100000002fb0000000800540069006d006500000000000000073a000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000780000003e000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1920 + X: 0 + Y: 0 diff --git a/control_ws/src/lotti_control3/bringup/launch/camera.launch.py b/control_ws/src/lotti_control3/bringup/launch/camera.launch.py new file mode 100644 index 0000000..b8c79bf --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/launch/camera.launch.py @@ -0,0 +1,36 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +def generate_launch_description(): + device_arg = LaunchConfiguration('device') + camera_name_arg = LaunchConfiguration('camera_name') + + return LaunchDescription([ + # Declare the arguments (with safe defaults if we forget to pass them) + DeclareLaunchArgument( + 'device', + default_value='/dev/video0', + description='Path to the video device port' + ), + DeclareLaunchArgument( + 'camera_name', + default_value='camera', + description='Namespace to prevent topic collisions' + ), + + Node( + package='webcam_publisher', + executable='ffmpeg_camera_node', + namespace=camera_name_arg, + name='ffmpeg_camera', + output='screen', + parameters=[{ + 'device': device_arg, + 'width': 640, + 'height': 480, + 'fps': 30 + }] + ) + ]) \ No newline at end of file diff --git a/control_ws/src/lotti_control3/bringup/launch/full.launch.py b/control_ws/src/lotti_control3/bringup/launch/full.launch.py new file mode 100644 index 0000000..2d4901a --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/launch/full.launch.py @@ -0,0 +1,207 @@ +import os +import yaml + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def load_servo_params(package_name, file_path, overrides=None): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + with open(absolute_file_path, "r", encoding="utf-8") as file: + raw_config = yaml.safe_load(file) or {} + + servo_config = dict(raw_config.get("moveit_servo", raw_config)) + if overrides: + servo_config.update(overrides) + + return {"moveit_servo": servo_config} + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically in full debug mode.", + ), + DeclareLaunchArgument( + "enable_drive_controller", + default_value="true", + description=( + "Start the tracked drive controller. " + "Full launch is intended for single-machine debug and keeps this enabled by default." + ), + ), + DeclareLaunchArgument( + "enable_flipper_controller", + default_value="true", + description=( + "Start the flipper controller. " + "Full launch is intended for single-machine debug and keeps this enabled by default." + ), + ), + ] + + gui = LaunchConfiguration("gui") + enable_drive_controller = LaunchConfiguration("enable_drive_controller") + enable_flipper_controller = LaunchConfiguration("enable_flipper_controller") + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "description", + "urdf", + "Lotti.urdf.xacro", + ]), + ]) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "config", + "lotti_controllers.yaml", + ]) + + rviz_config_file = PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "config", + "view_lotti.rviz", + ]) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["arm_controller", "-c", "/controller_manager"], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + drive_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["drive_controller", "-c", "/controller_manager"], + condition=IfCondition(enable_drive_controller), + ) + + flipper_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["flipper3_controller", "-c", "/controller_manager"], + condition=IfCondition(enable_flipper_controller), + ) + + delay_joint_state_broadcaster = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + + delay_drive_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[drive_controller_spawner], + ) + ) + + delay_flipper_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[flipper_controller_spawner], + ) + ) + + moveit_config = ( + MoveItConfigsBuilder("lotti3") + .robot_description(file_path="config/Lotti3.urdf.xacro") + .to_moveit_configs() + ) + + servo_node = Node( + package="moveit_servo", + executable="servo_node_main", + parameters=[ + load_servo_params("lotti_control3", "config/lotti_servo_config.yaml"), + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + delay_servo_node = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[servo_node], + ) + ) + + joy_node = Node( + package="joy", + executable="joy_node", + output="screen", + ) + + teleop_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("lotti_teleop"), + "launch", + "teleop_launch.py", + ) + ) + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + nodes = [ + control_node, + robot_state_pub_node, + arm_controller_spawner, + delay_joint_state_broadcaster, + delay_drive_controller, + delay_flipper_controller, + delay_servo_node, + joy_node, + teleop_launch, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/control_ws/src/lotti_control3/bringup/launch/gazebo.launch.py b/control_ws/src/lotti_control3/bringup/launch/gazebo.launch.py new file mode 100644 index 0000000..7923735 --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/launch/gazebo.launch.py @@ -0,0 +1,260 @@ +import os +import yaml + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + IncludeLaunchDescription, + RegisterEventHandler, +) +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit, OnProcessStart +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def load_servo_params(package_name, file_path, overrides=None): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + with open(absolute_file_path, "r", encoding="utf-8") as file: + raw_config = yaml.safe_load(file) or {} + + servo_config = dict(raw_config.get("moveit_servo", raw_config)) + if overrides: + servo_config.update(overrides) + + return {"moveit_servo": servo_config} + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "gui", + default_value="true", + description="Open the Gazebo GUI if the selected Gazebo launch supports it.", + ), + DeclareLaunchArgument( + "use_rviz", + default_value="true", + description="Start RViz2 automatically.", + ), + DeclareLaunchArgument( + "world", + default_value=PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "description", + "worlds", + "lotti_world.world", + ]), + description="Path to the Gazebo world file.", + ), + ] + + use_rviz = LaunchConfiguration("use_rviz") + world = LaunchConfiguration("world") + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "description", + "urdf", + "Lotti.urdf.xacro", + ]), + " use_sim:=true", + ]) + robot_description = {"robot_description": robot_description_content} + + gz_sim = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare("ros_gz_sim"), + "launch", + "gz_sim.launch.py", + ]) + ]), + launch_arguments={ + "gz_args": [world, " -r"], + "on_exit_shutdown": "true", + }.items(), + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description, {"use_sim_time": True}], + ) + + spawn_entity_node = Node( + package="ros_gz_sim", + executable="create", + arguments=[ + "-topic", "robot_description", + "-name", "lotti3", + "-x", "0.0", + "-y", "0.0", + "-z", "0.15", + ], + output="screen", + ) + + delay_spawn = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=robot_state_pub_node, + on_start=[spawn_entity_node], + ) + ) + + clock_bridge = Node( + package="ros_gz_bridge", + executable="parameter_bridge", + arguments=["/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock"], + output="screen", + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["arm_controller", "-c", "/controller_manager"], + parameters=[{"use_sim_time": True}], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + parameters=[{"use_sim_time": True}], + ) + + drive_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["drive_controller", "-c", "/controller_manager"], + parameters=[{"use_sim_time": True}], + ) + + flipper_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["flipper3_controller", "-c", "/controller_manager"], + parameters=[{"use_sim_time": True}], + ) + + delay_arm_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=spawn_entity_node, + on_exit=[arm_controller_spawner], + ) + ) + + delay_joint_state_broadcaster = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + + delay_drive_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[drive_controller_spawner], + ) + ) + + delay_flipper_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[flipper_controller_spawner], + ) + ) + + moveit_config = ( + MoveItConfigsBuilder("lotti3") + .robot_description(file_path="config/Lotti3.urdf.xacro") + .to_moveit_configs() + ) + + servo_node = Node( + package="moveit_servo", + executable="servo_node_main", + parameters=[ + load_servo_params( + "lotti_control3", + "config/lotti_servo_config.yaml", + overrides={"use_gazebo": True}, + ), + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + {"use_sim_time": True}, + ], + output="screen", + ) + + delay_servo_node = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_broadcaster_spawner, + on_exit=[servo_node], + ) + ) + + joy_node = Node( + package="joy", + executable="joy_node", + output="screen", + ) + + teleop_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("lotti_teleop"), + "launch", + "teleop_launch.py", + ) + ) + ) + + rviz_config_file = PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "config", + "view_lotti.rviz", + ]) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(use_rviz), + parameters=[{"use_sim_time": True}], + ) + + nodes = [ + gz_sim, + robot_state_pub_node, + clock_bridge, + delay_spawn, + delay_arm_controller, + delay_joint_state_broadcaster, + delay_drive_controller, + delay_flipper_controller, + delay_servo_node, + joy_node, + teleop_launch, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/control_ws/src/lotti_control3/bringup/launch/operator.launch.py b/control_ws/src/lotti_control3/bringup/launch/operator.launch.py new file mode 100644 index 0000000..56212b6 --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/launch/operator.launch.py @@ -0,0 +1,95 @@ +import os +import yaml + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription +from launch.conditions import IfCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +from ament_index_python import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder + + +def load_servo_params(package_name, file_path, overrides=None): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + with open(absolute_file_path, "r", encoding="utf-8") as file: + raw_config = yaml.safe_load(file) or {} + + servo_config = dict(raw_config.get("moveit_servo", raw_config)) + if overrides: + servo_config.update(overrides) + + return {"moveit_servo": servo_config} + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically on the operator station.", + ), + ] + + gui = LaunchConfiguration("gui") + rviz_config_file = PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "config", + "view_lotti.rviz", + ]) + + moveit_config = ( + MoveItConfigsBuilder("lotti3") + .robot_description(file_path="config/Lotti3.urdf.xacro") + .to_moveit_configs() + ) + + servo_node = Node( + package="moveit_servo", + executable="servo_node_main", + parameters=[ + load_servo_params("lotti_control3", "config/lotti_servo_config.yaml"), + moveit_config.robot_description, + moveit_config.robot_description_semantic, + moveit_config.robot_description_kinematics, + ], + output="screen", + ) + + joy_node = Node( + package="joy", + executable="joy_node", + output="screen", + ) + + teleop_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join( + get_package_share_directory("lotti_teleop"), + "launch", + "teleop_launch.py", + ) + ) + ) + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + nodes = [ + rviz_node, + servo_node, + joy_node, + teleop_launch, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/control_ws/src/lotti_control3/bringup/launch/robot.launch.py b/control_ws/src/lotti_control3/bringup/launch/robot.launch.py new file mode 100644 index 0000000..1894231 --- /dev/null +++ b/control_ws/src/lotti_control3/bringup/launch/robot.launch.py @@ -0,0 +1,121 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "enable_drive_controller", + default_value="false", + description=( + "Start the tracked drive controller on the robot PC. " + "Keep this disabled until the real Unitree transport is validated on Jazzy." + ), + ), + DeclareLaunchArgument( + "enable_flipper_controller", + default_value="false", + description=( + "Start the flipper controller on the robot PC. " + "Keep this disabled until the serial transport is validated on Jazzy." + ), + ), + ] + + enable_drive_controller = LaunchConfiguration("enable_drive_controller") + enable_flipper_controller = LaunchConfiguration("enable_flipper_controller") + + robot_description_content = Command([ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "description", + "urdf", + "Lotti.urdf.xacro", + ]), + ]) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution([ + FindPackageShare("lotti_control3"), + "config", + "lotti_controllers.yaml", + ]) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + arm_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["arm_controller", "-c", "/controller_manager"], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + drive_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["drive_controller", "-c", "/controller_manager"], + condition=IfCondition(enable_drive_controller), + ) + + flipper_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["flipper3_controller", "-c", "/controller_manager"], + condition=IfCondition(enable_flipper_controller), + ) + + delay_joint_state_broadcaster = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + + delay_drive_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[drive_controller_spawner], + ) + ) + + delay_flipper_controller = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=arm_controller_spawner, + on_exit=[flipper_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + arm_controller_spawner, + delay_joint_state_broadcaster, + delay_drive_controller, + delay_flipper_controller, + ] + + return LaunchDescription(declared_arguments + nodes) diff --git a/control_ws/src/lotti_control3/description/launch/view_lotti.launch.py b/control_ws/src/lotti_control3/description/launch/view_lotti.launch.py new file mode 100644 index 0000000..18aa616 --- /dev/null +++ b/control_ws/src/lotti_control3/description/launch/view_lotti.launch.py @@ -0,0 +1,85 @@ +# Copyright 2023 ros2_control Development Team +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start Rviz2 and Joint State Publisher gui automatically \ + with this launch file.", + ) + ) + # Initialize Arguments + gui = LaunchConfiguration("gui") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("lotti_control3"), + "description", + "urdf", + "Lotti.urdf.xacro", + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + rviz_config_file = PathJoinSubstitution( + [FindPackageShare("lotti_control3"), "config", "view_lotti.rviz"] + ) + + joint_state_publisher_node = Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + condition=IfCondition(gui), + ) + + robot_state_publisher_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + # + nodes_to_start = [ + joint_state_publisher_node, + robot_state_publisher_node, + rviz_node, + ] + + return LaunchDescription(declared_arguments + nodes_to_start) diff --git a/control_ws/src/lotti_control3/description/meshes/arm_link1_link.STL b/control_ws/src/lotti_control3/description/meshes/arm_link1_link.STL new file mode 100644 index 0000000..9559325 Binary files /dev/null and b/control_ws/src/lotti_control3/description/meshes/arm_link1_link.STL differ diff --git a/control_ws/src/lotti_control3/description/meshes/arm_link2_link.STL b/control_ws/src/lotti_control3/description/meshes/arm_link2_link.STL new file mode 100644 index 0000000..312d99d Binary files /dev/null and b/control_ws/src/lotti_control3/description/meshes/arm_link2_link.STL differ diff --git a/control_ws/src/lotti_control3/description/meshes/arm_link3_link.STL b/control_ws/src/lotti_control3/description/meshes/arm_link3_link.STL new file mode 100644 index 0000000..644638a Binary files /dev/null and b/control_ws/src/lotti_control3/description/meshes/arm_link3_link.STL differ diff --git a/control_ws/src/lotti_control3/description/meshes/arm_link4_link.STL b/control_ws/src/lotti_control3/description/meshes/arm_link4_link.STL new file mode 100644 index 0000000..e842358 Binary files /dev/null and b/control_ws/src/lotti_control3/description/meshes/arm_link4_link.STL differ diff --git a/control_ws/src/lotti_control3/description/meshes/arm_link5_link.STL b/control_ws/src/lotti_control3/description/meshes/arm_link5_link.STL new file mode 100644 index 0000000..e3f41f1 Binary files /dev/null and b/control_ws/src/lotti_control3/description/meshes/arm_link5_link.STL differ diff --git a/control_ws/src/lotti_control3/description/meshes/base_link.STL b/control_ws/src/lotti_control3/description/meshes/base_link.STL new file mode 100644 index 0000000..2553aa9 Binary files /dev/null and b/control_ws/src/lotti_control3/description/meshes/base_link.STL differ diff --git a/control_ws/src/lotti_control3/description/ros2_control/Lotti.ros2_control.xacro b/control_ws/src/lotti_control3/description/ros2_control/Lotti.ros2_control.xacro new file mode 100644 index 0000000..18a79de --- /dev/null +++ b/control_ws/src/lotti_control3/description/ros2_control/Lotti.ros2_control.xacro @@ -0,0 +1,98 @@ + + + + + + + arm3_interface/ArmInterface + /dev/serial/by-id/usb-ROBOTIS_OpenCR_Virtual_ComPort_in_FS_Mode_FFFFFFFEFFFF-if00 + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + + + flipper3_interface/FlipperInterface + /dev/serial/by-id/usb-Arduino__www.arduino.cc__0043_035363832363516043D2-if00 + write_only + + + + + + + + + + + + + + + + + + + + + + + + + + + + drive3_interface/DriveInterface + /dev/serial/by-id/usb-FTDI_USB__-__Serial_Converter_FTA362Q0-if00-port0 + 6 + 2 + 1 + 6.33 + + + + + + + + + + + + diff --git a/control_ws/src/lotti_control3/description/ros2_control/Lotti_sim.ros2_control.xacro b/control_ws/src/lotti_control3/description/ros2_control/Lotti_sim.ros2_control.xacro new file mode 100644 index 0000000..e7475b8 --- /dev/null +++ b/control_ws/src/lotti_control3/description/ros2_control/Lotti_sim.ros2_control.xacro @@ -0,0 +1,100 @@ + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + 0.0 + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + gz_ros2_control/GazeboSimSystem + + + + + + + + + + + + + + + + diff --git a/control_ws/src/lotti_control3/description/srdf/Lotti.srdf.xacro b/control_ws/src/lotti_control3/description/srdf/Lotti.srdf.xacro new file mode 100644 index 0000000..19c5737 --- /dev/null +++ b/control_ws/src/lotti_control3/description/srdf/Lotti.srdf.xacro @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/control_ws/src/lotti_control3/description/srdf/lotti_arm_group.xacro b/control_ws/src/lotti_control3/description/srdf/lotti_arm_group.xacro new file mode 100644 index 0000000..6a3cf8e --- /dev/null +++ b/control_ws/src/lotti_control3/description/srdf/lotti_arm_group.xacro @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control_ws/src/lotti_control3/description/srdf/lotti_body_group.xacro b/control_ws/src/lotti_control3/description/srdf/lotti_body_group.xacro new file mode 100644 index 0000000..5247b7e --- /dev/null +++ b/control_ws/src/lotti_control3/description/srdf/lotti_body_group.xacro @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control_ws/src/lotti_control3/description/urdf/Lotti.urdf.xacro b/control_ws/src/lotti_control3/description/urdf/Lotti.urdf.xacro new file mode 100644 index 0000000..68fd14b --- /dev/null +++ b/control_ws/src/lotti_control3/description/urdf/Lotti.urdf.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/control_ws/src/lotti_control3/description/urdf/Lotti_arm3.xacro b/control_ws/src/lotti_control3/description/urdf/Lotti_arm3.xacro new file mode 100644 index 0000000..799bf0b --- /dev/null +++ b/control_ws/src/lotti_control3/description/urdf/Lotti_arm3.xacro @@ -0,0 +1,291 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/control_ws/src/lotti_control3/description/urdf/Lotti_body.xacro b/control_ws/src/lotti_control3/description/urdf/Lotti_body.xacro new file mode 100644 index 0000000..5f36fe5 --- /dev/null +++ b/control_ws/src/lotti_control3/description/urdf/Lotti_body.xacro @@ -0,0 +1,160 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/control_ws/src/lotti_control3/description/urdf/Lotti_gazebo_control.xacro b/control_ws/src/lotti_control3/description/urdf/Lotti_gazebo_control.xacro new file mode 100644 index 0000000..91d0c65 --- /dev/null +++ b/control_ws/src/lotti_control3/description/urdf/Lotti_gazebo_control.xacro @@ -0,0 +1,56 @@ + + + + + + 0.9 + 0.9 + + + + 1.0 + 1.0 + + + + 1.0 + 1.0 + + + + + 0.2 + 0.2 + + + + 0.2 + 0.2 + + + + 0.2 + 0.2 + + + + 0.2 + 0.2 + + + + + 0.2 + 0.2 + + + + + + $(find lotti_control3)/config/lotti_controllers.yaml + + + + diff --git a/control_ws/src/lotti_control3/description/urdf/Lotti_lidar.xacro b/control_ws/src/lotti_control3/description/urdf/Lotti_lidar.xacro new file mode 100644 index 0000000..cdd4fb2 --- /dev/null +++ b/control_ws/src/lotti_control3/description/urdf/Lotti_lidar.xacro @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/control_ws/src/lotti_control3/description/urdf/urdf_inertial_shapes.xacro b/control_ws/src/lotti_control3/description/urdf/urdf_inertial_shapes.xacro new file mode 100644 index 0000000..3099c2a --- /dev/null +++ b/control_ws/src/lotti_control3/description/urdf/urdf_inertial_shapes.xacro @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/control_ws/src/lotti_control3/description/urdf/urdf_materials.xacro b/control_ws/src/lotti_control3/description/urdf/urdf_materials.xacro new file mode 100644 index 0000000..2675a78 --- /dev/null +++ b/control_ws/src/lotti_control3/description/urdf/urdf_materials.xacro @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/control_ws/src/lotti_control3/description/worlds/lotti_world.world b/control_ws/src/lotti_control3/description/worlds/lotti_world.world new file mode 100644 index 0000000..f195f69 --- /dev/null +++ b/control_ws/src/lotti_control3/description/worlds/lotti_world.world @@ -0,0 +1,38 @@ + + + + + + + model://sun + + + + + model://ground_plane + + + + + 1000.0 + 0.001 + 1 + + + quick + 150 + 0 + 1.400000 + 1 + + + 0.00001 + 0.2 + 2000.000000 + 0.01000 + + + + + + diff --git a/control_ws/src/lotti_control3/hardware/arm3_interface.cpp b/control_ws/src/lotti_control3/hardware/arm3_interface.cpp new file mode 100644 index 0000000..cdc68ce --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/arm3_interface.cpp @@ -0,0 +1,179 @@ +#include "lotti_control3/arm3_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace { + +rclcpp::Clock & throttle_clock(){ + static rclcpp::Clock clock(RCL_STEADY_TIME); + return clock; +} + +} // namespace + +namespace arm3_interface{ + CallbackReturn ArmInterface::on_init(const hardware_interface::HardwareInfo & info){ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS){ + return CallbackReturn::ERROR; + } + + //get the Arduino ID from the ros2_control file + device_ = info_.hardware_parameters["device"]; + + // robot has 5 joints, 1 interfaces + joint_positions_.assign(5, 0); + joint_positions_command_.assign(5, 0); + + for (const auto & joint : info_.joints){ + for (const auto & interface : joint.state_interfaces){ + joint_interfaces[interface.name].push_back(joint.name); + } + } + + return CallbackReturn::SUCCESS; + } + + std::vector ArmInterface::export_state_interfaces(){ + std::vector state_interfaces; + + int ind = 0; + for (const auto &joint_name : joint_interfaces["position"]){ + state_interfaces.emplace_back(joint_name, "position", &joint_positions_[ind++]); + } + + return state_interfaces; + } + + std::vector ArmInterface::export_command_interfaces(){ + std::vector command_interfaces; + + int ind = 0; + for (const auto &joint_name : joint_interfaces["position"]){ + command_interfaces.emplace_back(joint_name, "position", &joint_positions_command_[ind++]); + } + + return command_interfaces; + } + + hardware_interface::CallbackReturn ArmInterface::on_configure(const rclcpp_lifecycle::State & previous_state){ + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Configuring ...please wait..."); + + if (arm_comms_.connected()){ + arm_comms_.disconnect(); + } + arm_comms_.connect(device_); + + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Successfully configured!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn ArmInterface::on_cleanup(const rclcpp_lifecycle::State & previous_state){ + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Cleaning up ...please wait..."); + + if (arm_comms_.connected()){ + arm_comms_.disconnect(); + } + + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Successfully cleaned up!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn ArmInterface::on_activate(const rclcpp_lifecycle::State & previous_state){ + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Activating ...please wait..."); + + if (!arm_comms_.connected()){ + RCLCPP_ERROR(rclcpp::get_logger("ArmInterface"), "Arduino not connected"); + return hardware_interface::CallbackReturn::ERROR; + } + + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Successfully activated!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn ArmInterface::on_deactivate(const rclcpp_lifecycle::State & previous_state){ + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Deactivating ...please wait..."); + + for (auto i = 0ul; i < joint_positions_command_.size(); i++){ + com_pos[i] = 0.01; + } + arm_comms_.set_arm_values(com_pos); + + RCLCPP_INFO(rclcpp::get_logger("ArmInterface"), "Successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + return_type ArmInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period){ + if (!arm_comms_.connected()){ + return hardware_interface::return_type::ERROR; + } + + std::string arm_answer_ = arm_comms_.read_msg(); + if (arm_answer_.empty()) { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("ArmInterface"), + throttle_clock(), + 2000, + "Arm serial transport timed out without a feedback line. Keeping the last known joint positions." + ); + return return_type::OK; + } + + const int parsed_values = std::sscanf( + arm_answer_.c_str(), + "%i:%i:%i:%i:%i", + &state_pos_[0], + &state_pos_[1], + &state_pos_[2], + &state_pos_[3], + &state_pos_[4] + ); + + if (parsed_values != 5) { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("ArmInterface"), + throttle_clock(), + 2000, + "Received malformed arm feedback '%s'. Keeping the last known joint positions.", + arm_answer_.c_str() + ); + return return_type::OK; + } + + for (auto i = 0ul; i < joint_positions_.size(); i++){ + joint_positions_[i] = (float(state_pos_[i]) * 2 * 3.1416) / 4096; + } + + // The real feedback from Arduino is now being forwarded to ROS 2 correctly! + + return return_type::OK; + } + + return_type ArmInterface::write(const rclcpp::Time &, const rclcpp::Duration &){ + + for (auto i = 0ul; i < joint_positions_command_.size(); i++){ + com_pos[i] = joint_positions_command_[i]; + } + arm_comms_.set_arm_values(com_pos); + + return return_type::OK; + } + +} // namespace arm3_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + arm3_interface::ArmInterface, hardware_interface::SystemInterface) diff --git a/control_ws/src/lotti_control3/hardware/drive3_interface.cpp b/control_ws/src/lotti_control3/hardware/drive3_interface.cpp new file mode 100644 index 0000000..eac156b --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/drive3_interface.cpp @@ -0,0 +1,289 @@ +#include "lotti_control3/drive3_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +#include "unitreeMotor/unitreeMotor.h" + + +namespace { + +rclcpp::Clock & throttle_clock(){ + static rclcpp::Clock clock(RCL_STEADY_TIME); + return clock; +} + +int get_optional_int_parameter( + const hardware_interface::HardwareInfo & info, + const std::string & key, + const int fallback) +{ + const auto parameter = info.hardware_parameters.find(key); + if (parameter == info.hardware_parameters.end() || parameter->second.empty()) { + return fallback; + } + + return std::stoi(parameter->second); +} + +float get_optional_float_parameter( + const hardware_interface::HardwareInfo & info, + const std::string & key, + const float fallback) +{ + const auto parameter = info.hardware_parameters.find(key); + if (parameter == info.hardware_parameters.end() || parameter->second.empty()) { + return fallback; + } + + return std::stof(parameter->second); +} + +unsigned short resolve_drive_mode(){ +#if LOTTI_HAVE_UNITREE_SDK + return static_cast(queryMotorMode(MotorType::GO_M8010_6, MotorMode::FOC)); +#else + return 1; +#endif +} + +} // namespace + +namespace drive3_interface{ + CallbackReturn DriveInterface::on_init(const hardware_interface::HardwareInfo &info){ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS){ + return CallbackReturn::ERROR; + } + + // Get the configured serial device and drive tuning values from ros2_control. + device_ = info_.hardware_parameters["device"]; + max_speed_ = get_optional_int_parameter(info_, "max_speed", 6); + left_motor_id_ = get_optional_int_parameter(info_, "left_motor_id", 2); + right_motor_id_ = get_optional_int_parameter(info_, "right_motor_id", 1); + gearRatio = get_optional_float_parameter(info_, "gear_ratio", 6.33F); + + + // robot has 2 joints, 1 interface + joint_velocities_command_.assign(2, 0); + joint_velocities_.assign(2,0); + + for (const auto &joint : info_.joints){ + for (const auto &interface : joint.state_interfaces){ + joint_interfaces[interface.name].push_back(joint.name); + } + } + return CallbackReturn::SUCCESS; + } + + std::vector DriveInterface::export_state_interfaces(){ + std::vector state_interfaces; + + int ind = 0; + for (const auto &joint_name : joint_interfaces["velocity"]){ + state_interfaces.emplace_back(joint_name, "velocity", &joint_velocities_[ind++]); + } + return state_interfaces; + } + + std::vector DriveInterface::export_command_interfaces(){ + std::vector command_interfaces; + + for (int ind = 0; ind < 2; ind++){ + command_interfaces.emplace_back(info_.joints[ind].name, "velocity", &joint_velocities_command_[ind]); + } + return command_interfaces; + } + + hardware_interface::CallbackReturn DriveInterface::on_configure(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Configuring ...please wait..."); + + drive_transport_ready_ = false; + drive_feedback_valid_ = false; + +#if LOTTI_HAVE_UNITREE_SDK + try { + serial_port_ = std::make_unique(device_); + drive_transport_ready_ = true; + gearRatio = queryGearRatio(MotorType::GO_M8010_6); + + RCLCPP_INFO( + rclcpp::get_logger("DriveInterface"), + "Drive transport ready on %s with gear ratio %.3f", + device_.c_str(), + gearRatio + ); + } catch (const std::exception & exception) { + serial_port_.reset(); + RCLCPP_ERROR( + rclcpp::get_logger("DriveInterface"), + "Failed to initialize the Unitree drive transport on %s: %s", + device_.c_str(), + exception.what() + ); + } catch (...) { + serial_port_.reset(); + RCLCPP_ERROR( + rclcpp::get_logger("DriveInterface"), + "Failed to initialize the Unitree drive transport on %s due to an unknown error", + device_.c_str() + ); + } +#else + RCLCPP_WARN( + rclcpp::get_logger("DriveInterface"), + "Drive transport is disabled because lotti_control3 was built without the optional vendor Unitree SDK." + ); +#endif + + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Successfully configured"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn DriveInterface::on_cleanup(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Cleaning up ...please wait..."); + + drive_transport_ready_ = false; + drive_feedback_valid_ = false; +#if LOTTI_HAVE_UNITREE_SDK + serial_port_.reset(); +#endif + + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Successfully cleaned up!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn DriveInterface::on_activate(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Activating ...please wait..."); + + if (!drive_transport_ready_) { + RCLCPP_WARN( + rclcpp::get_logger("DriveInterface"), + "DriveInterface is active without a real motor transport. Leave the drive controller disabled unless the SDK-backed transport is available." + ); + } + + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Successfully activated"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn DriveInterface::on_deactivate(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Deactivating ...please wait..."); + + joint_velocities_command_[0] = 0.0; + joint_velocities_command_[1] = 0.0; + drive_feedback_valid_ = false; + write(rclcpp::Time{}, rclcpp::Duration::from_seconds(0.0)); + + RCLCPP_INFO(rclcpp::get_logger("DriveInterface"), "Successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + return_type DriveInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & period){ + if (!drive_feedback_valid_) { + if (drive_transport_ready_) { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("DriveInterface"), + throttle_clock(), + 2000, + "Drive transport is up but no valid motor feedback has been received yet." + ); + } + + return return_type::OK; + } + + if (data_l_.correct) { + joint_velocities_[0] = data_l_.dq / gearRatio; + } else { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("DriveInterface"), + throttle_clock(), + 2000, + "Drive transport reported invalid feedback for the left motor." + ); + } + + if (data_r_.correct) { + joint_velocities_[1] = data_r_.dq / gearRatio; + } else { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("DriveInterface"), + throttle_clock(), + 2000, + "Drive transport reported invalid feedback for the right motor." + ); + } + + return return_type::OK; + } + + return_type DriveInterface::write(const rclcpp::Time & /*time*/, const rclcpp::Duration &){ + + speed_l = joint_velocities_command_[0] * max_speed_ * gearRatio; // directly from command interface + speed_r = joint_velocities_command_[1] * max_speed_ * gearRatio; + + // Set commands + cmd_l_.motorType = MotorType::GO_M8010_6; + data_l_.motorType = MotorType::GO_M8010_6; + cmd_l_.mode = resolve_drive_mode(); + cmd_l_.id = static_cast(left_motor_id_); + cmd_l_.kp = 0.0; + cmd_l_.kd = 0.05; + cmd_l_.q = 0.0; + cmd_l_.dq = speed_l; + cmd_l_.tau = 0.0; + + cmd_r_.motorType = MotorType::GO_M8010_6; + data_r_.motorType = MotorType::GO_M8010_6; + cmd_r_.mode = resolve_drive_mode(); + cmd_r_.id = static_cast(right_motor_id_); + cmd_r_.kp = 0.0; + cmd_r_.kd = 0.05; + cmd_r_.q = 0.0; + cmd_r_.dq = speed_r; + cmd_r_.tau = 0.0; + +#if LOTTI_HAVE_UNITREE_SDK + if (drive_transport_ready_ && serial_port_) { + const bool left_ok = serial_port_->sendRecv(&cmd_l_, &data_l_); + const bool right_ok = serial_port_->sendRecv(&cmd_r_, &data_r_); + drive_feedback_valid_ = left_ok && right_ok && data_l_.correct && data_r_.correct; + + if (!drive_feedback_valid_) { + RCLCPP_ERROR_THROTTLE( + rclcpp::get_logger("DriveInterface"), + throttle_clock(), + 2000, + "Drive command exchange failed or returned invalid feedback." + ); + } + } else { + drive_feedback_valid_ = false; + } +#else + drive_feedback_valid_ = false; +#endif + + return return_type::OK; + } + +} // namespace drive3_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + drive3_interface::DriveInterface, hardware_interface::SystemInterface) diff --git a/control_ws/src/lotti_control3/hardware/flipper3_interface.cpp b/control_ws/src/lotti_control3/hardware/flipper3_interface.cpp new file mode 100644 index 0000000..8140e5b --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/flipper3_interface.cpp @@ -0,0 +1,189 @@ +#include "lotti_control3/flipper3_interface.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware_interface/lexical_casts.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace { + +rclcpp::Clock & throttle_clock(){ + static rclcpp::Clock clock(RCL_STEADY_TIME); + return clock; +} + +int clamp_flipper_command(const double raw_command){ + const auto rounded = static_cast(std::lround(raw_command)); + return std::max(-1, std::min(rounded, 1)); +} + +} // namespace + +namespace flipper3_interface{ + CallbackReturn FlipperInterface::on_init(const hardware_interface::HardwareInfo &info){ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS){ + return CallbackReturn::ERROR; + } + + // Get the configured serial device from the ros2_control file. + device_ = info_.hardware_parameters["device"]; + const auto feedback_mode = info_.hardware_parameters.find("feedback_mode"); + if (feedback_mode != info_.hardware_parameters.end() && !feedback_mode->second.empty()) { + feedback_mode_ = feedback_mode->second; + } + + // robot has 4 joints, 2 interfaces + joint_positions_.assign(4, 0); + joint_velocities_command_.assign(4, 0); + + for (const auto &joint : info_.joints){ + for (const auto &interface : joint.state_interfaces){ + joint_interfaces[interface.name].push_back(joint.name); + } + } + return CallbackReturn::SUCCESS; + } + + std::vector FlipperInterface::export_state_interfaces(){ + std::vector state_interfaces; + + int ind = 0; + for (const auto &joint_name : joint_interfaces["position"]){ + state_interfaces.emplace_back(joint_name, "position", &joint_positions_[ind++]); + } + return state_interfaces; + } + + std::vector FlipperInterface::export_command_interfaces(){ + std::vector command_interfaces; + + for (int ind = 0; ind < 4; ind++){ + command_interfaces.emplace_back(info_.joints[ind].name, "velocity", &joint_velocities_command_[ind]); + } + return command_interfaces; + } + + hardware_interface::CallbackReturn FlipperInterface::on_configure(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Configuring ...please wait..."); + + flipper_transport_ready_ = false; + + try { + if (flipper_comms_.connected()){ + flipper_comms_.disconnect(); + } + flipper_comms_.connect(device_); + flipper_transport_ready_ = flipper_comms_.connected(); + } catch (const std::exception & exception) { + RCLCPP_ERROR( + rclcpp::get_logger("FlipperInterface"), + "Failed to initialize the flipper serial transport on %s: %s", + device_.c_str(), + exception.what() + ); + } catch (...) { + RCLCPP_ERROR( + rclcpp::get_logger("FlipperInterface"), + "Failed to initialize the flipper serial transport on %s due to an unknown error", + device_.c_str() + ); + } + + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Successfully configured"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn FlipperInterface::on_cleanup(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Cleaning up ...please wait..."); + + if (flipper_comms_.connected()){ + flipper_comms_.disconnect(); + } + flipper_transport_ready_ = false; + + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Successfully cleaned up!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn FlipperInterface::on_activate(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Activating ...please wait..."); + + flipper_transport_ready_ = flipper_comms_.connected(); + + if (!flipper_transport_ready_){ + RCLCPP_WARN( + rclcpp::get_logger("FlipperInterface"), + "FlipperInterface is active without a serial connection. Leave the flipper controller disabled until the Arduino transport is available." + ); + } else if (feedback_mode_ == "write_only") { + RCLCPP_WARN( + rclcpp::get_logger("FlipperInterface"), + "Flipper transport is connected in write-only mode. Joint states remain latched because the current Arduino firmware does not publish encoder feedback." + ); + } + + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Successfully activated"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + hardware_interface::CallbackReturn FlipperInterface::on_deactivate(const rclcpp_lifecycle::State &previous_state){ + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Deactivating ...please wait..."); + + joint_velocities_command_.assign(4, 0.0); + write(rclcpp::Time{}, rclcpp::Duration::from_seconds(0.0)); + + RCLCPP_INFO(rclcpp::get_logger("FlipperInterface"), "Successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; + } + + return_type FlipperInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/){ + if (!flipper_transport_ready_) { + return return_type::OK; + } + + if (feedback_mode_ == "write_only") { + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("FlipperInterface"), + throttle_clock(), + 5000, + "FlipperInterface is running with write-only Arduino firmware. Joint positions stay at their last reported values until real feedback is added." + ); + return return_type::OK; + } + + return return_type::OK; + } + + return_type FlipperInterface::write(const rclcpp::Time & /*time*/, const rclcpp::Duration &){ + if (!flipper_transport_ready_) { + return return_type::OK; + } + + // The controller already presents commands in logical FL/FR/RL/RR order. + flipper_comms_.set_flipper_values( + clamp_flipper_command(joint_velocities_command_[0]), + clamp_flipper_command(joint_velocities_command_[1]), + clamp_flipper_command(joint_velocities_command_[2]), + clamp_flipper_command(joint_velocities_command_[3]) + ); + + return return_type::OK; + } + +} // namespace flipper3_interface + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + flipper3_interface::FlipperInterface, hardware_interface::SystemInterface) diff --git a/control_ws/src/lotti_control3/hardware/include/lotti_control3/arm3_interface.hpp b/control_ws/src/lotti_control3/hardware/include/lotti_control3/arm3_interface.hpp new file mode 100644 index 0000000..9b9cb9f --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/include/lotti_control3/arm3_interface.hpp @@ -0,0 +1,67 @@ +#ifndef ARM3_INTERFACE__ARM3_INTERFACE_HPP_ +#define ARM3_INTERFACE__ARM3_INTERFACE_HPP_ + +#include "string" +#include "unordered_map" +#include "vector" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "lotti_control3/serial_comms.hpp" + +using hardware_interface::return_type; + +namespace arm3_interface{ + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class HARDWARE_INTERFACE_PUBLIC ArmInterface : public hardware_interface::SystemInterface { + public: + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + + + protected: + /// The size of this vector is (standard_interfaces_.size() x nr_joints) + std::vector joint_positions_command_; + std::vector joint_positions_; + + std::vector com_pos = {0.0, 0.0, 0.0, 0.0, 0.0}; + + int state_pos_[5] = {0, 0, 0, 0, 0}; + std::string device_ = ""; + + std::unordered_map> joint_interfaces = { + {"position", {}} + }; + +//- + SerialComms arm_comms_; + + }; + +} // namespace arm3_interface + +#endif // ARM3_INTERFACE__ARM3_INTERFACE_HPP_ diff --git a/control_ws/src/lotti_control3/hardware/include/lotti_control3/drive3_interface.hpp b/control_ws/src/lotti_control3/hardware/include/lotti_control3/drive3_interface.hpp new file mode 100644 index 0000000..0f1ba13 --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/include/lotti_control3/drive3_interface.hpp @@ -0,0 +1,88 @@ + +#ifndef DRIVE3_INTERFACE__DRIVE3_INTERFACE_HPP_ +#define DRIVE3_INTERFACE__DRIVE3_INTERFACE_HPP_ + +#include "string" +#include "memory" +#include "unordered_map" +#include "vector" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "unitreeMotor/unitreeMotor.h" + +#ifndef LOTTI_HAVE_UNITREE_SDK +#define LOTTI_HAVE_UNITREE_SDK 0 +#endif + +#if LOTTI_HAVE_UNITREE_SDK +#include "serialPort/SerialPort.h" +#endif + +using hardware_interface::return_type; + +namespace drive3_interface{ + + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class HARDWARE_INTERFACE_PUBLIC DriveInterface : public hardware_interface::SystemInterface { + + public: + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + + protected: + /// The size of this vector is (standard_interfaces_.size() x nr_joints) + std::vector joint_velocities_command_; + std::vector joint_velocities_; + + std::string device_ = ""; + int max_speed_ = 0; + float gearRatio = 6.33; + int left_motor_id_ = 2; + int right_motor_id_ = 1; + bool drive_transport_ready_ = false; + bool drive_feedback_valid_ = false; + + float speed_l = 0.0; + float speed_r = 0.0; + + std::unordered_map> joint_interfaces = { + //{"position", {}}, + {"velocity", {}}}; + + // --- Add Unitree SDK members here --- + MotorCmd cmd_l_, cmd_r_; // Motor commands for left and right motors + MotorData data_l_, data_r_; // Motor data feedback for left and right motors + +#if LOTTI_HAVE_UNITREE_SDK + std::unique_ptr serial_port_; +#endif + }; +} // namespace drive3_interface + +#endif // DRIVE_INTERFACE__DRIVE_INTERFACE_HPP_ diff --git a/control_ws/src/lotti_control3/hardware/include/lotti_control3/flipper3_interface.hpp b/control_ws/src/lotti_control3/hardware/include/lotti_control3/flipper3_interface.hpp new file mode 100644 index 0000000..cc7eba4 --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/include/lotti_control3/flipper3_interface.hpp @@ -0,0 +1,74 @@ +#ifndef FLIPPER3_INTERFACE__FLIPPER3_INTERFACE_HPP_ +#define FLIPPER3_INTERFACE__FLIPPER3_INTERFACE_HPP_ + +#include "string" +#include "unordered_map" +#include "vector" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/logger.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "lotti_control3/serial_comms.hpp" + +using hardware_interface::return_type; + +namespace flipper3_interface{ + + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + + class HARDWARE_INTERFACE_PUBLIC FlipperInterface : public hardware_interface::SystemInterface { + + public: + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & previous_state) override; + + hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + + protected: + /// The size of this vector is (standard_interfaces_.size() x nr_joints) + std::vector joint_velocities_command_; + std::vector joint_positions_; + + std::string device_ = ""; + std::string feedback_mode_ = "write_only"; + bool flipper_transport_ready_ = false; + + float fr_cmd_ = 0; + float fl_cmd_ = 0; + float rr_cmd_ = 0; + float rl_cmd_ = 0; + + float fr_state_ = 0; + float fl_state_ = 0; + float rr_state_ = 0; + float rl_state_ = 0; + + std::unordered_map> joint_interfaces = { + {"position", {}}, {"velocity", {}}}; + + SerialComms flipper_comms_; + }; +} // namespace flipper3_interface + +#endif // FLIPPER_INTERFACE__FLIPPER_INTERFACE_HPP_ diff --git a/control_ws/src/lotti_control3/hardware/include/lotti_control3/serial_comms.hpp b/control_ws/src/lotti_control3/hardware/include/lotti_control3/serial_comms.hpp new file mode 100644 index 0000000..66cdf27 --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/include/lotti_control3/serial_comms.hpp @@ -0,0 +1,81 @@ +#ifndef lotti_control3_SERIAL_COMMS_HPP +#define lotti_control3_SERIAL_COMMS_HPP + +#include +#include +#include +#include "libserial/SerialPort.h" +#include +#include "sstream" + + +class SerialComms{ + + public: + + SerialComms() = default; + + void connect(const std::string &serial_device){ + timeout_ms_ = 1000; + serial_conn_.Open(serial_device); + serial_conn_.SetBaudRate(LibSerial::BaudRate::BAUD_115200); + } + + + void disconnect(){ + serial_conn_.Close(); + } + + + bool connected() const{ + return serial_conn_.IsOpen(); + } + + + void send_msg(const std::string &msg_to_send){ + serial_conn_.FlushIOBuffers(); // Just in case + serial_conn_.Write(msg_to_send); + } + + + void send_empty_msg(){ + send_msg("\n"); + } + + + std::string read_msg(){ + std::string response = ""; + + try{ + serial_conn_.ReadLine(response, '\n', timeout_ms_); + } + catch (const LibSerial::ReadTimeout&){ + std::cerr << "The ReadByte() call has timed out." << std::endl ; + } + return response; + } + + + void set_arm_values(std::vector pos){ + std::stringstream ss; + for (size_t i = 0; i < 5; i++){ + ss << pos[i] << ":"; + } + ss << "\n"; + //std::cout << ss.str(); + send_msg(ss.str()); + } + + void set_flipper_values(int FL, int FR, int RL, int RR){ + std::stringstream ss; + // The Arduino flipper firmware expects logical board order FL/FR/RL/RR. + ss << "FL" << FL << "FR" << FR << "RL" << RL << "RR" << RR << "\n"; + send_msg(ss.str()); + } + + private: + LibSerial::SerialPort serial_conn_; + int timeout_ms_; +}; + +#endif // lotti_control3_SERIAL_COMMS_HPP diff --git a/control_ws/src/lotti_control3/hardware/test.cpp b/control_ws/src/lotti_control3/hardware/test.cpp new file mode 100644 index 0000000..c0afcc3 --- /dev/null +++ b/control_ws/src/lotti_control3/hardware/test.cpp @@ -0,0 +1,28 @@ + +#include +#include "serialPort/SerialPort.h" +#include "unitreeMotor/unitreeMotor.h" + +class test { + public: + + + void send(double speedPercent){ + //SerialPort serial("/dev/ttyACM0"); + MotorCmd cmd; + MotorData data; + double speedR = speedPercent * 6; + + cmd.motorType = MotorType::GO_M8010_6; + data.motorType = MotorType::GO_M8010_6; + //cmd.mode = queryMotorMode(MotorType::GO_M8010_6,MotorMode::FOC); + cmd.id = 1; + cmd.kp = 0.0; + cmd.kd = 0.05; + cmd.q = 0.0; + //cmd.dq = speedR*queryGearRatio(MotorType::GO_M8010_6); + cmd.tau = 0.0; + //serial.sendRecv(&cmd,&data); + std::cout << speedR*queryGearRatio(MotorType::GO_M8010_6); + } +}; \ No newline at end of file diff --git a/control_ws/src/lotti_control3/include/IOPort/IOPort.h b/control_ws/src/lotti_control3/include/IOPort/IOPort.h new file mode 100644 index 0000000..173b617 --- /dev/null +++ b/control_ws/src/lotti_control3/include/IOPort/IOPort.h @@ -0,0 +1,41 @@ +#ifndef __IOPORT_H +#define __IOPORT_H + +#include +#include +#include +#include "unitreeMotor/unitreeMotor.h" + +enum class BlockYN{ + YES, + NO +}; + +class IOPort{ +public: + IOPort(BlockYN blockYN, size_t recvLength, size_t timeOutUs){ + resetIO(blockYN, recvLength, timeOutUs); + } + virtual ~IOPort(){} + virtual size_t send(uint8_t *sendMsg, size_t sendLength) = 0; + virtual size_t recv(uint8_t *recvMsg, size_t recvLength) = 0; + virtual size_t recv(uint8_t *recvMsg) = 0; + virtual bool sendRecv(std::vector &sendVec, std::vector &recvVec) = 0; + void resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs); +protected: + BlockYN _blockYN = BlockYN::NO; + size_t _recvLength; + timeval _timeout; + timeval _timeoutSaved; +}; + +inline void IOPort::resetIO(BlockYN blockYN, size_t recvLength, size_t timeOutUs){ + _blockYN = blockYN; + _recvLength = recvLength; + _timeout.tv_sec = timeOutUs / 1000000; + _timeout.tv_usec = timeOutUs % 1000000; + _timeoutSaved = _timeout; +} + + +#endif // z1_lib_IOPORT_H \ No newline at end of file diff --git a/control_ws/src/lotti_control3/include/crc/crc32.h b/control_ws/src/lotti_control3/include/crc/crc32.h new file mode 100644 index 0000000..cea99c8 --- /dev/null +++ b/control_ws/src/lotti_control3/include/crc/crc32.h @@ -0,0 +1,33 @@ +#ifndef CRC32_H +#define CRC32_H + +#include + +inline uint32_t crc32_core(uint32_t* ptr, uint32_t len){ + uint32_t xbit = 0; + uint32_t data = 0; + uint32_t CRC32 = 0xFFFFFFFF; + const uint32_t dwPolynomial = 0x04c11db7; + for (uint32_t i = 0; i < len; i++) + { + xbit = 1 << 31; + data = ptr[i]; + for (uint32_t bits = 0; bits < 32; bits++) + { + if (CRC32 & 0x80000000) + { + CRC32 <<= 1; + CRC32 ^= dwPolynomial; + } + else + CRC32 <<= 1; + if (data & xbit) + CRC32 ^= dwPolynomial; + + xbit >>= 1; + } + } + return CRC32; +} + +#endif // CRC32_H \ No newline at end of file diff --git a/control_ws/src/lotti_control3/include/crc/crc_ccitt.h b/control_ws/src/lotti_control3/include/crc/crc_ccitt.h new file mode 100644 index 0000000..2930e24 --- /dev/null +++ b/control_ws/src/lotti_control3/include/crc/crc_ccitt.h @@ -0,0 +1,67 @@ +#ifndef __CRC_CCITT_H +#define __CRC_CCITT_H + +/* + * This mysterious table is just the CRC of each possible byte. It can be + * computed using the standard bit-at-a-time methods. The polynomial can + * be seen in entry 128, 0x8408. This corresponds to x^0 + x^5 + x^12. + * Add the implicit x^16, and you have the standard CRC-CCITT. + * https://github.com/torvalds/linux/blob/5bfc75d92efd494db37f5c4c173d3639d4772966/lib/crc-ccitt.c + */ +uint16_t const crc_ccitt_table[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, + 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, + 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, + 0x9cc9, 0x8d40, 0xbfdb, 0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, + 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, + 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, + 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, + 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb, + 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, + 0x5285, 0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, + 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, + 0x6306, 0x728f, 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, + 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, + 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, 0x9af9, 0x8b70, + 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, + 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff, + 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, + 0x18c1, 0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, + 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, + 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, 0xd0bd, 0xc134, + 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, + 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1, 0xa33a, 0xb2b3, + 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, + 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, + 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, + 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, + 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, + 0x7bc7, 0x6a4e, 0x58d5, 0x495c, 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; + + +static uint16_t crc_ccitt_byte(uint16_t crc, const uint8_t c) +{ + return (crc >> 8) ^ crc_ccitt_table[(crc ^ c) & 0xff]; +} + +/** + * crc_ccitt - recompute the CRC (CRC-CCITT variant) for the data + * buffer + * @crc: previous CRC value + * @buffer: data pointer + * @len: number of bytes in the buffer + */ +inline uint16_t crc_ccitt(uint16_t crc, uint8_t const *buffer, size_t len) +{ + while (len--) + crc = crc_ccitt_byte(crc, *buffer++); + return crc; +} + + +#endif diff --git a/control_ws/src/lotti_control3/include/serialPort/SerialPort.h b/control_ws/src/lotti_control3/include/serialPort/SerialPort.h new file mode 100644 index 0000000..0f608b2 --- /dev/null +++ b/control_ws/src/lotti_control3/include/serialPort/SerialPort.h @@ -0,0 +1,98 @@ +#ifndef __SERIALPORT_H +#define __SERIALPORT_H + +/* +High frequency serial communication, +Not that common, but useful for motor communication. +*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "serialPort/include/errorClass.h" +#include "unitreeMotor/unitreeMotor.h" +#include "IOPort/IOPort.h" + +enum class bytesize_t{ + fivebits, + sixbits, + sevenbits, + eightbits +}; + +enum class parity_t{ + parity_none, + parity_odd, + parity_even, + parity_mark, + parity_space +}; + +enum class stopbits_t{ + stopbits_one, + stopbits_two, + stopbits_one_point_five +}; + +enum class flowcontrol_t{ + flowcontrol_none, + flowcontrol_software, + flowcontrol_hardware +}; + +class SerialPort : public IOPort{ +public: + SerialPort(const std::string &portName, + size_t recvLength = 16, + uint32_t baudrate = 4000000, + size_t timeOutUs = 20000, + BlockYN blockYN = BlockYN::NO, + bytesize_t bytesize = bytesize_t::eightbits, + parity_t parity = parity_t::parity_none, + stopbits_t stopbits = stopbits_t::stopbits_one, + flowcontrol_t flowcontrol = flowcontrol_t::flowcontrol_none); + virtual ~SerialPort(); + void resetSerial(size_t recvLength = 16, + uint32_t baudrate = 4000000, + size_t timeOutUs = 20000, + BlockYN blockYN = BlockYN::NO, + bytesize_t bytesize = bytesize_t::eightbits, + parity_t parity = parity_t::parity_none, + stopbits_t stopbits = stopbits_t::stopbits_one, + flowcontrol_t flowcontrol = flowcontrol_t::flowcontrol_none); + size_t send(uint8_t *sendMsg, size_t sendLength); + size_t recv(uint8_t *recvMsg, size_t recvLength); + size_t recv(uint8_t *recvMsg); + bool sendRecv(uint8_t *sendMsg, uint8_t *recvMsg, size_t sendLength); + bool sendRecv(MotorCmd* sendMsg, MotorData* recvMsg); + bool sendRecv(std::vector &sendVec, std::vector &recvVec); + void test(); +private: + void _open(); + void _set(); + void _close(); + size_t _nonBlockRecv(uint8_t *recvMsg, size_t readLen); + std::string _portName; + uint32_t _baudrate; + bytesize_t _bytesize; + parity_t _parity; + stopbits_t _stopbits; + flowcontrol_t _flowcontrol; + bool _xonxoff; + bool _rtscts; + int _fd; + fd_set _rSet; + +}; + + + + +#endif // SERIALPORT_H \ No newline at end of file diff --git a/control_ws/src/lotti_control3/include/serialPort/include/errorClass.h b/control_ws/src/lotti_control3/include/serialPort/include/errorClass.h new file mode 100644 index 0000000..48313c3 --- /dev/null +++ b/control_ws/src/lotti_control3/include/serialPort/include/errorClass.h @@ -0,0 +1,52 @@ +#ifndef __ERRORCLASS_H +#define __ERRORCLASS_H + +#include +#include +#include +#include +#include + +#define THROW(exceptionClass, message) throw exceptionClass(__FILE__, \ +__LINE__, (message) ) + +class IOException : public std::exception +{ + // Disable copy constructors + IOException& operator=(const IOException&); + std::string file_; + int line_; + std::string e_what_; + int errno_; +public: + explicit IOException (std::string file, int line, int errnum) + : file_(file), line_(line), errno_(errnum) { + std::stringstream ss; +#if defined(_WIN32) && !defined(__MINGW32__) + char error_str [1024]; + strerror_s(error_str, 1024, errnum); +#else + char * error_str = strerror(errnum); +#endif + ss << "IO Exception (" << errno_ << "): " << error_str; + ss << ", file " << file_ << ", line " << line_ << "."; + e_what_ = ss.str(); + } + explicit IOException (std::string file, int line, const char * description) + : file_(file), line_(line), errno_(0) { + std::stringstream ss; + ss << "IO Exception: " << description; + ss << ", file " << file_ << ", line " << line_ << "."; + e_what_ = ss.str(); + } + virtual ~IOException() throw() {} + IOException (const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {} + + int getErrorNumber () const { return errno_; } + + virtual const char* what () const throw () { + return e_what_.c_str(); + } +}; + +#endif // ERRORCLASS_H \ No newline at end of file diff --git a/control_ws/src/lotti_control3/include/unitreeMotor/include/motor_msg_A1B1.h b/control_ws/src/lotti_control3/include/unitreeMotor/include/motor_msg_A1B1.h new file mode 100644 index 0000000..95120ab --- /dev/null +++ b/control_ws/src/lotti_control3/include/unitreeMotor/include/motor_msg_A1B1.h @@ -0,0 +1,162 @@ +#ifndef MOTOR_A1B1_MSG +#define MOTOR_A1B1_MSG + +#include +typedef int16_t q15_t; + +#pragma pack(1) + +// 发送用单个数据数据结构 +typedef union{ + int32_t L; + uint8_t u8[4]; + uint16_t u16[2]; + uint32_t u32; + float F; +}COMData32; + +typedef struct { + // 定义 数据包头 + unsigned char start[2]; // 包头 + unsigned char motorID; // 电机ID 0,1,2,3 ... 0xBB 表示向所有电机广播（此时无返回） + unsigned char reserved; +}COMHead; + +#pragma pack() + +#pragma pack(1) + +typedef struct { + + uint8_t fan_d; // 关节上的散热风扇转速 + uint8_t Fmusic; // 电机发声频率 /64*1000 15.625f 频率分度 + uint8_t Hmusic; // 电机发声强度 推荐值4 声音强度 0.1 分度 + uint8_t reserved4; + + uint8_t FRGB[4]; // 足端LED + +}LowHzMotorCmd; + +typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整 + // 定义 数据 + uint8_t mode; // 关节模式选择 + uint8_t ModifyBit; // 电机控制参数修改位 + uint8_t ReadBit; // 电机控制参数发送位 + uint8_t reserved; + + COMData32 Modify; // 电机参数修改 的数据 + //实际给FOC的指令力矩为： + //K_P*delta_Pos + K_W*delta_W + T + q15_t T; // 期望关节的输出力矩（电机本身的力矩）x256, 7 + 8 描述 + q15_t W; // 期望关节速度 （电机本身的速度） x128, 8 + 7描述 + int32_t Pos; // 期望关节位置 x 16384/6.2832, 14位编码器（主控0点修正，电机关节还是以编码器0点为准） + + q15_t K_P; // 关节刚度系数 x2048 4+11 描述 + q15_t K_W; // 关节速度系数 x1024 5+10 描述 + + uint8_t LowHzMotorCmdIndex; // 电机低频率控制命令的索引, 0-7, 分别代表LowHzMotorCmd中的8个字节 + uint8_t LowHzMotorCmdByte; // 电机低频率控制命令的字节 + + COMData32 Res[1]; // 通讯 保留字节 用于实现别的一些通讯内容 + +}MasterComdV3; // 加上数据包的包头 和CRC 34字节 + +typedef struct { + // 定义 电机控制命令数据包 + COMHead head; + MasterComdV3 Mdata; + COMData32 CRCdata; +}MasterComdDataV3;//返回数据 + +// typedef struct { +// // 定义 总得485 数据包 + +// MasterComdData M1; +// MasterComdData M2; +// MasterComdData M3; + +// }DMA485TxDataV3; + +#pragma pack() + +#pragma pack(1) + +typedef struct { // 以 4个字节一组排列 ，不然编译器会凑整 + // 定义 数据 + uint8_t mode; // 当前关节模式 + uint8_t ReadBit; // 电机控制参数修改 是否成功位 + int8_t Temp; // 电机当前平均温度 + uint8_t MError; // 电机错误 标识 + + COMData32 Read; // 读取的当前 电机 的控制数据 + int16_t T; // 当前实际电机输出力矩 7 + 8 描述 + + int16_t W; // 当前实际电机速度（高速） 8 + 7 描述 + float LW; // 当前实际电机速度（低速） + + int16_t W2; // 当前实际关节速度（高速） 8 + 7 描述 + float LW2; // 当前实际关节速度（低速） + + int16_t Acc; // 电机转子加速度 15+0 描述 惯量较小 + int16_t OutAcc; // 输出轴加速度 12+3 描述 惯量较大 + + int32_t Pos; // 当前电机位置（主控0点修正，电机关节还是以编码器0点为准） + int32_t Pos2; // 关节编码器位置(输出编码器) + + int16_t gyro[3]; // 电机驱动板6轴传感器数据 + int16_t acc[3]; + + // 力传感器的数据 + int16_t Fgyro[3]; // + int16_t Facc[3]; + int16_t Fmag[3]; + uint8_t Ftemp; // 8位表示的温度 7位（-28~100度） 1位0.5度分辨率 + + int16_t Force16; // 力传感器高16位数据 + int8_t Force8; // 力传感器低8位数据 + + uint8_t FError; // 足端传感器错误标识 + + int8_t Res[1]; // 通讯 保留字节 + +}ServoComdV3; // 加上数据包的包头 和CRC 78字节（4+70+4） + +typedef struct { + // 定义 电机控制命令数据包 + COMHead head; + ServoComdV3 Mdata; + + COMData32 CRCdata; + +}ServoComdDataV3; //发送数据 + +// typedef struct { +// // 定义 总的485 接受数据包 + +// ServoComdDataV3 M[3]; +// // uint8_t nullbyte1; + +// }DMA485RxDataV3; + + +#pragma pack() + +// 00 00 00 00 00 +// 00 00 00 00 00 +// 00 00 00 00 00 +// 00 00 00 +// 数据包默认初始化 +// 主机发送的数据包 +/* + Tx485Data[_FR][i].head.start[0] = 0xFE ; Tx485Data[_FR][i].head.start[1] = 0xEE; // 数据包头 + Tx485Data[_FR][i].Mdata.ModifyBit = 0xFF; Tx485Data[_FR][i].Mdata.mode = 0; // 默认不修改数据 和 电机的默认工作模式 + Tx485Data[_FR][i].head.motorID = i; 0 // 目标电机标号 + Tx485Data[_FR][i].Mdata.T = 0.0f; // 默认目标关节输出力矩 motor1.Extra_Torque = motorRxData[1].Mdata.T*0.390625f; // N.M 转化为 N.CM IQ8描述 + Tx485Data[_FR][i].Mdata.Pos = 0x7FE95C80; // 默认目标关节位置 不启用位置环 14位分辨率 + Tx485Data[_FR][i].Mdata.W = 16000.0f; // 默认目标关节速度 不启用速度环 1+8+7描述 motor1.Target_Speed = motorRxData[1].Mdata.W*0.0078125f; // 单位 rad/s IQ7描述 + Tx485Data[_FR][i].Mdata.K_P = (q15_t)(0.6f*(1<<11)); // 默认关节刚度系数 4+11 描述 motor1.K_Pos = ((float)motorRxData[1].Mdata.K_P)/(1<<11); // 描述刚度的通讯数据格式 4+11 + Tx485Data[_FR][i].Mdata.K_W = (q15_t)(1.0f*(1<<10)); // 默认关节速度系数 5+10 描述 motor1.K_Speed = ((float)motorRxData[1].Mdata.K_W)/(1<<10); // 描述阻尼的通讯数据格式 5+10 +*/ + + +#endif \ No newline at end of file diff --git a/control_ws/src/lotti_control3/include/unitreeMotor/include/motor_msg_GO-M8010-6.h b/control_ws/src/lotti_control3/include/unitreeMotor/include/motor_msg_GO-M8010-6.h new file mode 100644 index 0000000..54aeefe --- /dev/null +++ b/control_ws/src/lotti_control3/include/unitreeMotor/include/motor_msg_GO-M8010-6.h @@ -0,0 +1,90 @@ +#ifndef __MOTOR_MSG_GO_M8010_6_H +#define __MOTOR_MSG_GO_M8010_6_H + +#include +#define CRC_SIZE 2 +#define CTRL_DAT_SIZE sizeof(ControlData_t) - CRC_SIZE +#define DATA_DAT_SIZE sizeof(MotorData_t) - CRC_SIZE + +#pragma pack(1) + +/** + * @brief 电机模式控制信息 + * + */ +typedef struct +{ + uint8_t id :4; // 电机ID: 0,1...,14 15表示向所有电机广播数据(此时无返回) + uint8_t status :3; // 工作模式: 0.锁定 1.FOC闭环 2.编码器校准 3.保留 + uint8_t none :1; // 保留位 +} RIS_Mode_t; // 控制模式 1Byte + +/** + * @brief 电机状态控制信息 + * + */ +typedef struct +{ + int16_t tor_des; // 期望关节输出扭矩 unit: N.m (q8) + int16_t spd_des; // 期望关节输出速度 unit: rad/s (q7) + int32_t pos_des; // 期望关节输出位置 unit: rad (q15) + uint16_t k_pos; // 期望关节刚度系数 unit: 0.0-1.0 (q15) + uint16_t k_spd; // 期望关节阻尼系数 unit: 0.0-1.0 (q15) + +} RIS_Comd_t; // 控制参数 12Byte + +/** + * @brief 电机状态反馈信息 + * + */ +typedef struct +{ + int16_t torque; // 实际关节输出扭矩 unit: N.m (q8) + int16_t speed; // 实际关节输出速度 unit: rad/s (q7) + int32_t pos; // 实际关节输出位置 unit: W (q15) + int8_t temp; // 电机温度: -128~127°C 90°C时触发温度保护 + uint8_t MError :3; // 电机错误标识: 0.正常 1.过热 2.过流 3.过压 4.编码器故障 5-7.保留 + uint16_t force :12; // 足端气压传感器数据 12bit (0-4095) + uint8_t none :1; // 保留位 +} RIS_Fbk_t; // 状态数据 11Byte + + +#pragma pack() + +#pragma pack(1) + +/** + * @brief 控制数据包格式 + * + */ +typedef struct +{ + uint8_t head[2]; // 包头 2Byte + RIS_Mode_t mode; // 电机控制模式 1Byte + RIS_Comd_t comd; // 电机期望数据 12Byte + uint16_t CRC16; // CRC 2Byte + +} ControlData_t; // 主机控制命令 17Byte + +/** + * @brief 电机反馈数据包格式 + * + */ +typedef struct +{ + uint8_t head[2]; // 包头 2Byte + RIS_Mode_t mode; // 电机控制模式 1Byte + RIS_Fbk_t fbk; // 电机反馈数据 11Byte + uint16_t CRC16; // CRC 2Byte + +} MotorData_t; // 电机返回数据 16Byte + +#pragma pack() + +#endif + + + + + + diff --git a/control_ws/src/lotti_control3/include/unitreeMotor/unitreeMotor.h b/control_ws/src/lotti_control3/include/unitreeMotor/unitreeMotor.h new file mode 100644 index 0000000..fbce3de --- /dev/null +++ b/control_ws/src/lotti_control3/include/unitreeMotor/unitreeMotor.h @@ -0,0 +1,74 @@ +#ifndef __UNITREEMOTOR_H +#define __UNITREEMOTOR_H + +#include "unitreeMotor/include/motor_msg_GO-M8010-6.h" +#include "unitreeMotor/include/motor_msg_A1B1.h" +#include +#include + +enum class MotorType{ + A1, // 4.8M baudrate + B1, // 6.0M baudrate + GO_M8010_6 +}; + +enum class MotorMode{ + BRAKE, + FOC, + CALIBRATE +}; + +struct MotorCmd{ + public: + MotorCmd(){} + MotorType motorType; + int hex_len; + unsigned short id; + unsigned short mode; + float tau; + float dq; + float q; + float kp; + float kd; + + void modify_data(MotorCmd* motor_s); + uint8_t* get_motor_send_data(); + + COMData32 Res; + private: + ControlData_t GO_M8010_6_motor_send_data; + MasterComdDataV3 A1B1_motor_send_data; +}; + +struct MotorData{ + public: + MotorData(){} + MotorType motorType; + int hex_len; + unsigned char motor_id; + unsigned char mode; + int temp; + int merror; + float tau; + float dq; + float q; + + bool correct = false; + bool extract_data(MotorData* motor_r); + uint8_t* get_motor_recv_data(); + + int footForce; + float LW; + int Acc; + + float gyro[3]; + float acc[3]; + private: + MotorData_t GO_M8010_6_motor_recv_data; + ServoComdDataV3 A1B1_motor_recv_data; +}; + +// Utility Function +int queryMotorMode(MotorType motortype,MotorMode motormode); +float queryGearRatio(MotorType motortype); +#endif // UNITREEMOTOR_H \ No newline at end of file diff --git a/control_ws/src/lotti_control3/lotti_control3.xml b/control_ws/src/lotti_control3/lotti_control3.xml new file mode 100644 index 0000000..009861d --- /dev/null +++ b/control_ws/src/lotti_control3/lotti_control3.xml @@ -0,0 +1,28 @@ + + + + ros2_control hardware interface for the Lotti3 arm over serial transport. + + + + + + ros2_control hardware interface for the Lotti3 flippers over serial transport. The current firmware is command-only and therefore exposes limited state feedback. + + + + + + ros2_control hardware interface for the Lotti3 tracked drive. Real transport is available when the optional vendor Unitree SDK is enabled at build time. + + + diff --git a/control_ws/src/lotti_control3/package.xml b/control_ws/src/lotti_control3/package.xml new file mode 100644 index 0000000..1f428c0 --- /dev/null +++ b/control_ws/src/lotti_control3/package.xml @@ -0,0 +1,43 @@ + + + + lotti_control3 + 0.3.0 + + Hardware interfaces, URDF description, controllers, and bringup launch files for the Lotti3 rescue robot. + + Res.Q Bots + BSD + Res.Q Bots + + ament_cmake + + ament_index_python + controller_manager + gz_ros2_control + hardware_interface + joint_state_publisher_gui + joy + launch + launch_ros + libserial-dev + lotti_drive_controller + lotti_flipper3_controller + lotti_teleop + moveit_configs_utils + moveit_servo + pluginlib + python3-yaml + rclcpp + rclcpp_lifecycle + rcpputils + robot_state_publisher + ros_gz_bridge + ros_gz_sim + rviz2 + xacro + + + ament_cmake + + diff --git a/control_ws/src/lotti_drive_controller/CMakeLists.txt b/control_ws/src/lotti_drive_controller/CMakeLists.txt new file mode 100644 index 0000000..4a4173a --- /dev/null +++ b/control_ws/src/lotti_drive_controller/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(lotti_drive_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# set the same behavior for windows as it is on linux +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# find dependencies +set(CONTROLLER_INCLUDE_DEPENDS + pluginlib + rcpputils + controller_interface + geometry_msgs + rclcpp + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +#compile +add_library( + lotti_drive_controller + SHARED + lotti_drive_controller/drive_controller.cpp +) + +target_compile_features(lotti_drive_controller PUBLIC cxx_std_17) +target_include_directories(lotti_drive_controller PUBLIC +$ +$ +) +ament_target_dependencies( + lotti_drive_controller PUBLIC + ${CONTROLLER_INCLUDE_DEPENDS} +) + +# Export controller plugins +pluginlib_export_plugin_description_file(controller_interface lotti_drive_controller.xml) + +# INSTALL +install( + DIRECTORY lotti_drive_controller/include/ + DESTINATION include/lotti_drive_controller +) +install(TARGETS lotti_drive_controller + EXPORT export_lotti_drive_controller + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +## EXPORTS +ament_export_targets(export_lotti_drive_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS}) +ament_package() + diff --git a/botlaunch/LICENSE b/control_ws/src/lotti_drive_controller/LICENSE similarity index 100% rename from botlaunch/LICENSE rename to control_ws/src/lotti_drive_controller/LICENSE diff --git a/control_ws/src/lotti_drive_controller/lotti_drive_controller.xml b/control_ws/src/lotti_drive_controller/lotti_drive_controller.xml new file mode 100644 index 0000000..089f79b --- /dev/null +++ b/control_ws/src/lotti_drive_controller/lotti_drive_controller.xml @@ -0,0 +1,10 @@ + + + + Robot controller plugin for Lotti tracks + + + + diff --git a/control_ws/src/lotti_drive_controller/lotti_drive_controller/drive_controller.cpp b/control_ws/src/lotti_drive_controller/lotti_drive_controller/drive_controller.cpp new file mode 100644 index 0000000..4b6ce59 --- /dev/null +++ b/control_ws/src/lotti_drive_controller/lotti_drive_controller/drive_controller.cpp @@ -0,0 +1,120 @@ +#include "lotti_drive_controller/drive_controller.hpp" + +#include +#include +#include +#include +#include +#include "rclcpp/qos.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include + + +using config_type = controller_interface::interface_configuration_type; + +namespace drive_controller{ + DriveController::DriveController() : controller_interface::ControllerInterface() {} + + controller_interface::CallbackReturn DriveController::on_init(){ + // should have error handling + joint_names_ = auto_declare>("joints", joint_names_); + command_interface_types_ = + auto_declare>("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare>("state_interfaces", state_interface_types_); + + return CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration DriveController::command_interface_configuration() + const{ + controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; + + conf.names.reserve(joint_names_.size() * command_interface_types_.size()); + for (const auto &joint_name : joint_names_){ + for (const auto &interface_type : command_interface_types_){ + conf.names.push_back(joint_name + "/" + interface_type); + } + } + + return conf; + } + + controller_interface::InterfaceConfiguration DriveController::state_interface_configuration() const{ + controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; + + conf.names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto &joint_name : joint_names_){ + for (const auto &interface_type : state_interface_types_){ + conf.names.push_back(joint_name + "/" + interface_type); + } + } + + return conf; + } + + controller_interface::CallbackReturn DriveController::on_configure(const rclcpp_lifecycle::State &){ + auto drive_callback = + [this](geometry_msgs::msg::Twist drive_msg) -> void { + cmd_speed_ = drive_msg.linear.x; + cmd_angle_ = drive_msg.angular.z; + }; + + // Subscribers + drive_sub_ = + get_node()->create_subscription( + "/cmd/drive", 1, drive_callback); + + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn DriveController::on_activate(const rclcpp_lifecycle::State &){ + // clear out vectors in case of restart + joint_velocity_command_interface_.clear(); + + // assign command interfaces + for (auto & interface : command_interfaces_){ + command_interface_map_[interface.get_interface_name()]->push_back(interface); + } + + /* // assign state interfaces + for (auto & interface : state_interfaces_){ + state_interface_map_[interface.get_interface_name()]->push_back(interface); + } */ + + return CallbackReturn::SUCCESS; + } + + controller_interface::return_type DriveController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/){ + + if (cmd_speed_ < 0.0){ + left_wheel_cmd = std::max(-1.0f, std::min((cmd_speed_ + cmd_angle_), 1.0f)); + right_wheel_cmd = - std::max(-1.0f, std::min((cmd_speed_ - cmd_angle_), 1.0f)); + } + else{ + left_wheel_cmd = std::max(-1.0f, std::min((cmd_speed_ - cmd_angle_), 1.0f)); + right_wheel_cmd = - std::max(-1.0f, std::min((cmd_speed_ + cmd_angle_), 1.0f)); + } + + joint_velocity_command_interface_[0].get().set_value(left_wheel_cmd); + joint_velocity_command_interface_[1].get().set_value(right_wheel_cmd); + + + return controller_interface::return_type::OK; + } + + controller_interface::CallbackReturn DriveController::on_deactivate(const rclcpp_lifecycle::State &){ + release_interfaces(); + + return CallbackReturn::SUCCESS; + } + +} // namespace drive_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + drive_controller::DriveController, controller_interface::ControllerInterface) diff --git a/control_ws/src/lotti_drive_controller/lotti_drive_controller/include/lotti_drive_controller/drive_controller.hpp b/control_ws/src/lotti_drive_controller/lotti_drive_controller/include/lotti_drive_controller/drive_controller.hpp new file mode 100644 index 0000000..a727c0f --- /dev/null +++ b/control_ws/src/lotti_drive_controller/lotti_drive_controller/include/lotti_drive_controller/drive_controller.hpp @@ -0,0 +1,60 @@ +#ifndef LOTTI_DRIVE_CONTROLLER__DRIVE_CONTROLLER_HPP_ +#define LOTTI_DRIVE_CONTROLLER__DRIVE_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "geometry_msgs/msg/twist.hpp" + + +namespace drive_controller{ + class DriveController : public controller_interface::ControllerInterface{ + public: + DriveController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + protected: + std::vector joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + + rclcpp::Subscription::SharedPtr drive_sub_; + rclcpp::Time start_time_; + float cmd_speed_ = 0; + float cmd_angle_ = 0; + + float left_wheel_cmd = 0.0; + float right_wheel_cmd = 0.0; + + std::vector> + joint_velocity_command_interface_; + + std::unordered_map< + std::string, std::vector> *> + command_interface_map_ = { + {"velocity", &joint_velocity_command_interface_}}; + + }; + +} // namespace drive_controller + +#endif // LOTTI_DRIVE_CONTROLLER__DRIVE_CONTROLLER_HPP_ diff --git a/control_ws/src/lotti_drive_controller/package.xml b/control_ws/src/lotti_drive_controller/package.xml new file mode 100644 index 0000000..fc7bb79 --- /dev/null +++ b/control_ws/src/lotti_drive_controller/package.xml @@ -0,0 +1,27 @@ + + + + lotti_drive_controller + 0.0.0 + Custom ros2_control controller that maps Twist drive commands onto the Lotti3 tracked drive interfaces. + paul + Apache-2.0 + + ament_cmake + + controller_interface + geometry_msgs + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + + controller_manager + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/control_ws/src/lotti_flipper3_controller/CMakeLists.txt b/control_ws/src/lotti_flipper3_controller/CMakeLists.txt new file mode 100644 index 0000000..29d6162 --- /dev/null +++ b/control_ws/src/lotti_flipper3_controller/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.8) +project(lotti_flipper3_controller) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# set the same behavior for windows as it is on linux +set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) + +# find dependencies +set(CONTROLLER_INCLUDE_DEPENDS + pluginlib + rcpputils + controller_interface + std_msgs + rclcpp + rclcpp_lifecycle +) + +# find dependencies +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + + +#compile +add_library( + lotti_flipper3_controller + SHARED + lotti_flipper3_controller/flipper3_controller.cpp +) + +target_compile_features(lotti_flipper3_controller PUBLIC cxx_std_17) +target_include_directories(lotti_flipper3_controller PUBLIC +$ +$ +) +ament_target_dependencies( + lotti_flipper3_controller PUBLIC + ${CONTROLLER_INCLUDE_DEPENDS} +) + +# Export controller plugins +pluginlib_export_plugin_description_file(controller_interface lotti_flipper3_controller.xml) + +# INSTALL +install( + DIRECTORY lotti_flipper3_controller/include/ + DESTINATION include/lotti_flipper3_controller +) +install(TARGETS lotti_flipper3_controller + EXPORT export_lotti_flipper3_controller + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +## EXPORTS +ament_export_targets(export_lotti_flipper3_controller HAS_LIBRARY_TARGET) +ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS}) +ament_package() + diff --git a/control_ws/src/lotti_flipper3_controller/LICENSE b/control_ws/src/lotti_flipper3_controller/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/control_ws/src/lotti_flipper3_controller/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller.xml b/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller.xml new file mode 100644 index 0000000..86170d2 --- /dev/null +++ b/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller.xml @@ -0,0 +1,10 @@ + + + + Robot controller plugin for Lotti flippers + + + + diff --git a/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller/flipper3_controller.cpp b/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller/flipper3_controller.cpp new file mode 100644 index 0000000..4bf9a3c --- /dev/null +++ b/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller/flipper3_controller.cpp @@ -0,0 +1,143 @@ +#include "lotti_flipper3_controller/flipper3_controller.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include "rclcpp/qos.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include + + +using config_type = controller_interface::interface_configuration_type; + +namespace flipper3_controller{ + FlipperController::FlipperController() : controller_interface::ControllerInterface() {} + + controller_interface::CallbackReturn FlipperController::on_init(){ + // should have error handling + joint_names_ = auto_declare>("joints", joint_names_); + command_interface_types_ = + auto_declare>("command_interfaces", command_interface_types_); + state_interface_types_ = + auto_declare>("state_interfaces", state_interface_types_); + + return CallbackReturn::SUCCESS; + } + + controller_interface::InterfaceConfiguration FlipperController::command_interface_configuration() + const{ + controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; + + conf.names.reserve(joint_names_.size() * command_interface_types_.size()); + for (const auto &joint_name : joint_names_){ + for (const auto &interface_type : command_interface_types_){ + conf.names.push_back(joint_name + "/" + interface_type); + } + } + + return conf; + } + + controller_interface::InterfaceConfiguration FlipperController::state_interface_configuration() const{ + controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}}; + + conf.names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto &joint_name : joint_names_){ + for (const auto &interface_type : state_interface_types_){ + conf.names.push_back(joint_name + "/" + interface_type); + } + } + + return conf; + } + + controller_interface::CallbackReturn FlipperController::on_configure(const rclcpp_lifecycle::State &){ + auto fr_flipper_callback = + [this](std_msgs::msg::Int8 fr_flipper_msg) -> void { + fr_flipper_cmd = fr_flipper_msg.data; + }; + + auto fl_flipper_callback = + [this](std_msgs::msg::Int8 fl_flipper_msg) -> void { + fl_flipper_cmd = fl_flipper_msg.data; + }; + + auto rr_flipper_callback = + [this](std_msgs::msg::Int8 rr_flipper_msg) -> void { + rr_flipper_cmd = rr_flipper_msg.data; + }; + + auto rl_flipper_callback = + [this](std_msgs::msg::Int8 rl_flipper_msg) -> void { + rl_flipper_cmd = rl_flipper_msg.data; + }; + + // Subscribers + fr_flipper_sub_ = + get_node()->create_subscription( + "/cmd/flipper_fr", 1, fr_flipper_callback); + + fl_flipper_sub_ = + get_node()->create_subscription( + "/cmd/flipper_fl", 1, fl_flipper_callback); + + rr_flipper_sub_ = + get_node()->create_subscription( + "/cmd/flipper_rr", 1, rr_flipper_callback); + + rl_flipper_sub_ = + get_node()->create_subscription( + "/cmd/flipper_rl", 1, rl_flipper_callback); + + return CallbackReturn::SUCCESS; + } + + controller_interface::CallbackReturn FlipperController::on_activate(const rclcpp_lifecycle::State &){ + // clear out vectors in case of restart + joint_velocity_command_interface_.clear(); + joint_position_state_interface_.clear(); + + // assign command interfaces + for (auto & interface : command_interfaces_){ + command_interface_map_[interface.get_interface_name()]->push_back(interface); + } + + // assign state interfaces + for (auto & interface : state_interfaces_){ + state_interface_map_[interface.get_interface_name()]->push_back(interface); + } + + return CallbackReturn::SUCCESS; + } + + controller_interface::return_type FlipperController::update( + const rclcpp::Time & time, const rclcpp::Duration & /*period*/){ + + joint_velocity_command_interface_[0].get().set_value(fl_flipper_cmd); + joint_velocity_command_interface_[1].get().set_value(fr_flipper_cmd); + joint_velocity_command_interface_[2].get().set_value(rl_flipper_cmd); + joint_velocity_command_interface_[3].get().set_value(rr_flipper_cmd); + + //std::cout << std::to_string(fr_flipper_cmd) << "\n"; + return controller_interface::return_type::OK; + } + + controller_interface::CallbackReturn FlipperController::on_deactivate(const rclcpp_lifecycle::State &){ + release_interfaces(); + + return CallbackReturn::SUCCESS; + } + +} // namespace flipper3_controller + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + flipper3_controller::FlipperController, controller_interface::ControllerInterface) diff --git a/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller/include/lotti_flipper3_controller/flipper3_controller.hpp b/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller/include/lotti_flipper3_controller/flipper3_controller.hpp new file mode 100644 index 0000000..b41c61c --- /dev/null +++ b/control_ws/src/lotti_flipper3_controller/lotti_flipper3_controller/include/lotti_flipper3_controller/flipper3_controller.hpp @@ -0,0 +1,76 @@ +#ifndef LOTTI_FLIPPER3_CONTROLLER__FLIPPER3_CONTROLLER_HPP_ +#define LOTTI_FLIPPER3_CONTROLLER__FLIPPER3_CONTROLLER_HPP_ + +#include +#include +#include +#include + +#include "controller_interface/controller_interface.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/subscription.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/state.hpp" +#include "std_msgs/msg/int8.hpp" + + +namespace flipper3_controller{ + class FlipperController : public controller_interface::ControllerInterface{ + public: + FlipperController(); + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::return_type update( + const rclcpp::Time & time, const rclcpp::Duration & period) override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & previous_state) override; + + controller_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & previous_state) override; + + + + protected: + std::vector joint_names_; + std::vector command_interface_types_; + std::vector state_interface_types_; + + rclcpp::Subscription::SharedPtr fr_flipper_sub_; + rclcpp::Subscription::SharedPtr fl_flipper_sub_; + rclcpp::Subscription::SharedPtr rr_flipper_sub_; + rclcpp::Subscription::SharedPtr rl_flipper_sub_; + rclcpp::Time start_time_; + int fr_flipper_cmd = 0; + int fl_flipper_cmd = 0; + int rr_flipper_cmd = 0; + int rl_flipper_cmd = 0; + + std::vector> + joint_velocity_command_interface_; + std::vector> + joint_position_state_interface_; + + std::unordered_map< + std::string, std::vector> *> + command_interface_map_ = { + {"velocity", &joint_velocity_command_interface_}}; + + std::unordered_map< + std::string, std::vector> *> + state_interface_map_ = { + {"position", &joint_position_state_interface_}}; + }; + +} // namespace flipper3_controller + +#endif // LOTTI_FLIPPER_CONTROLLER__FLIPPER_CONTROLLER_HPP_ diff --git a/control_ws/src/lotti_flipper3_controller/package.xml b/control_ws/src/lotti_flipper3_controller/package.xml new file mode 100644 index 0000000..96142da --- /dev/null +++ b/control_ws/src/lotti_flipper3_controller/package.xml @@ -0,0 +1,27 @@ + + + + lotti_flipper3_controller + 0.0.0 + Custom ros2_control controller that forwards per-flipper commands to the Lotti3 flipper interfaces. + paul + Apache-2.0 + + ament_cmake + + controller_interface + pluginlib + rclcpp + rclcpp_lifecycle + rcpputils + std_msgs + + controller_manager + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/botlaunch/botlaunch/__init__.py b/control_ws/src/lotti_lidar_tf/lotti_lidar_tf/__init__.py similarity index 100% rename from botlaunch/botlaunch/__init__.py rename to control_ws/src/lotti_lidar_tf/lotti_lidar_tf/__init__.py diff --git a/control_ws/src/lotti_lidar_tf/lotti_lidar_tf/lidar_tf.py b/control_ws/src/lotti_lidar_tf/lotti_lidar_tf/lidar_tf.py new file mode 100644 index 0000000..d84a940 --- /dev/null +++ b/control_ws/src/lotti_lidar_tf/lotti_lidar_tf/lidar_tf.py @@ -0,0 +1,102 @@ +# Copyright 2021 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import math + +from geometry_msgs.msg import TransformStamped + +import numpy as np + +import rclpy +from rclpy.node import Node + +from tf2_ros import TransformBroadcaster + + +# This function is a stripped down version of the code in +# https://github.com/matthew-brett/transforms3d/blob/f185e866ecccb66c545559bc9f2e19cb5025e0ab/transforms3d/euler.py +# Besides simplifying it, this version also inverts the order to return x,y,z,w, which is +# the way that ROS prefers it. +def quaternion_from_euler(ai, aj, ak): + ai /= 2.0 + aj /= 2.0 + ak /= 2.0 + ci = math.cos(ai) + si = math.sin(ai) + cj = math.cos(aj) + sj = math.sin(aj) + ck = math.cos(ak) + sk = math.sin(ak) + cc = ci*ck + cs = ci*sk + sc = si*ck + ss = si*sk + + q = np.empty((4, )) + q[0] = cj*sc - sj*cs + q[1] = cj*ss + sj*cc + q[2] = cj*cs - sj*sc + q[3] = cj*cc + sj*ss + + return q + + +class StaticFramePublisher(Node): + """ + Broadcast transforms that never change. + + This example publishes transforms from `world` to a static turtle frame. + The transforms are published periodically using a timer to avoid getting lost. + """ + + def __init__(self): + super().__init__('static_robot_tf') + + self.tf_broadcaster = TransformBroadcaster(self) + + # Publish transforms constantly using a timer + self.timer = self.create_timer(0.1, self.make_transforms) + + def make_transforms(self): + t = TransformStamped() + + t.header.stamp = self.get_clock().now().to_msg() + t.header.frame_id = 'body_link' + t.child_frame_id = 'body' + + t.transform.translation.x = -0.28 + t.transform.translation.y = 0.0 + t.transform.translation.z = 0.3 + quat = quaternion_from_euler(0.0, (-3.1416/6), 0.0) + t.transform.rotation.x = quat[0] + t.transform.rotation.y = quat[1] + t.transform.rotation.z = quat[2] + t.transform.rotation.w = quat[3] + + self.tf_broadcaster.sendTransform(t) + + +def main(): + logger = rclpy.logging.get_logger('logger') + + + # pass parameters and initialize node + rclpy.init() + node = StaticFramePublisher() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + + rclpy.shutdown() diff --git a/chain_controller/package.xml b/control_ws/src/lotti_lidar_tf/package.xml similarity index 53% rename from chain_controller/package.xml rename to control_ws/src/lotti_lidar_tf/package.xml index 738cdb5..9f8329d 100644 --- a/chain_controller/package.xml +++ b/control_ws/src/lotti_lidar_tf/package.xml @@ -1,19 +1,18 @@ - chain_controller + lotti_lidar_tf 0.0.0 - TODO: Package description - resqbots - TODO: License declaration + Static TF broadcaster for the Lotti3 body and lidar frame alignment. + paul + Apache-2.0 + ament_python + + geometry_msgs + python3-numpy rclpy - time - math - numpy - sensor_msgs - std_msgs - threading + tf2_ros ament_copyright ament_flake8 diff --git a/botlaunch/resource/botlaunch b/control_ws/src/lotti_lidar_tf/resource/lotti_lidar_tf similarity index 100% rename from botlaunch/resource/botlaunch rename to control_ws/src/lotti_lidar_tf/resource/lotti_lidar_tf diff --git a/control_ws/src/lotti_lidar_tf/setup.cfg b/control_ws/src/lotti_lidar_tf/setup.cfg new file mode 100644 index 0000000..e0c1f4f --- /dev/null +++ b/control_ws/src/lotti_lidar_tf/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lotti_lidar_tf +[install] +install_scripts=$base/lib/lotti_lidar_tf diff --git a/botlaunch/setup.py b/control_ws/src/lotti_lidar_tf/setup.py similarity index 63% rename from botlaunch/setup.py rename to control_ws/src/lotti_lidar_tf/setup.py index d9b6446..d8951da 100644 --- a/botlaunch/setup.py +++ b/control_ws/src/lotti_lidar_tf/setup.py @@ -1,8 +1,6 @@ -import os -from glob import glob from setuptools import find_packages, setup -package_name = 'botlaunch' +package_name = 'lotti_lidar_tf' setup( name=package_name, @@ -12,17 +10,17 @@ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), ], install_requires=['setuptools'], zip_safe=True, - maintainer='resqbots', - maintainer_email='office@campus-robo.at', - description='TODO: Package description', + maintainer='paul', + maintainer_email='73643665+PaulKupka@users.noreply.github.com', + description='Static TF broadcaster for the Lotti3 body and lidar frame alignment.', license='Apache-2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ + 'body_tf = lotti_lidar_tf.lidar_tf:main', ], }, ) diff --git a/botlaunch/test/test_copyright.py b/control_ws/src/lotti_lidar_tf/test/test_copyright.py similarity index 100% rename from botlaunch/test/test_copyright.py rename to control_ws/src/lotti_lidar_tf/test/test_copyright.py diff --git a/botlaunch/test/test_flake8.py b/control_ws/src/lotti_lidar_tf/test/test_flake8.py similarity index 100% rename from botlaunch/test/test_flake8.py rename to control_ws/src/lotti_lidar_tf/test/test_flake8.py diff --git a/botlaunch/test/test_pep257.py b/control_ws/src/lotti_lidar_tf/test/test_pep257.py similarity index 100% rename from botlaunch/test/test_pep257.py rename to control_ws/src/lotti_lidar_tf/test/test_pep257.py diff --git a/control_ws/src/lotti_teleop/LICENSE b/control_ws/src/lotti_teleop/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/control_ws/src/lotti_teleop/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/control_ws/src/lotti_teleop/launch/teleop_launch.py b/control_ws/src/lotti_teleop/launch/teleop_launch.py new file mode 100644 index 0000000..35d08dd --- /dev/null +++ b/control_ws/src/lotti_teleop/launch/teleop_launch.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='lotti_teleop', + executable='teleop', + output='screen', + name='teleop', + arguments=['--ros-args', '--log-level', 'info'], + ), + ]) \ No newline at end of file diff --git a/chain_controller/chain_controller/__init__.py b/control_ws/src/lotti_teleop/lotti_teleop/__init__.py similarity index 100% rename from chain_controller/chain_controller/__init__.py rename to control_ws/src/lotti_teleop/lotti_teleop/__init__.py diff --git a/control_ws/src/lotti_teleop/lotti_teleop/lotti_teleop.py b/control_ws/src/lotti_teleop/lotti_teleop/lotti_teleop.py new file mode 100644 index 0000000..0c15095 --- /dev/null +++ b/control_ws/src/lotti_teleop/lotti_teleop/lotti_teleop.py @@ -0,0 +1,551 @@ +import rclpy +from rclpy.node import Node +import time + +from sensor_msgs.msg import Joy +from geometry_msgs.msg import Twist +from geometry_msgs.msg import TwistStamped +from control_msgs.msg import JointJog +from std_msgs.msg import Int8 +from threading import Lock +from std_srvs.srv import Trigger + + +# Global Defines -------------------------------------------------------------- +# mapping controller buttons and sticks to joy topc axes and buttons + +#axes +left_stick_x = 0 +left_stick_y = 1 +right_stick_x = 3 +right_stick_y = 4 +left_trigger = 2 +right_trigger = 5 +d_pad_x = 6 +d_pad_y = 7 +#buttons +a_button = 0 +b_button = 1 +x_button = 2 +y_button = 3 +lb_button = 4 +rb_button = 5 +opt_left_button = 6 +opt_right_button = 7 +xBox_button = 8 +left_stick_button = 9 +right_stick_button = 10 +space_button = 11 + +PI=3.1415926535897 + +# Class ----------------------------------------------------------------------- +class TeleOp(Node): + + def __init__(self): + #Entrypoint of the class + super().__init__('tele_op') + + #define variables + + #axes + self.__left_stick_x = float(0) + self.__left_stick_x_lock = Lock() + self.__left_stick_y = float(0) + self.__left_stick_y_lock = Lock() + + self.__left_trigger = float(0) + self.__left_trigger_lock = Lock() + + self.__right_stick_x = float(0) + self.__right_stick_x_lock = Lock() + self.__right_stick_y = float(0) + self.__right_stick_y_lock = Lock() + self.__right_stick_press = float(0) + self.__right_stick_press_lock = Lock() + + self.__right_trigger = float(0) + self.__right_trigger_lock = Lock() + + self.__d_pad_x = float(0) + self.__d_pad_x_lock = Lock() + + self.__d_pad_y = float(0) + self.__d_pad_y_lock = Lock() + + #buttons + self.__button_a = int(0) + self.__button_a_lock = Lock() + self.__button_b = int(0) + self.__button_b_lock = Lock() + self.__button_x = int(0) + self.__button_x_lock = Lock() + self.__button_y = int(0) + self.__button_y_lock = Lock() + + self.__button_rb = int(0) + self.__button_rb_lock = Lock() + + self.__button_lb = int(0) + self.__button_lb_lock = Lock() + + self.__button_opt_right = int(0) + self.__button_opt_right_lock = Lock() + + self.__button_opt_left = int(0) + self.__button_opt_left_lock = Lock() + + self.__button_space = int(0) + self.__button_space_lock = Lock() + + + #safety stuff + self.__first_joy_msg_received = bool(False) + self.__joy_enabled = bool(False) + self.__joy_enabled_old = bool(False) + self.__button_opt_right_pressed = bool(False) + self.__button_opt_right_last_pressed = 0.0 + + #switch to arm + self.__arm_enabled = bool(False) + self.__arm_enabled_old = bool(True) + self.__button_opt_left_pressed = bool(False) + self.__button_opt_left_last_pressed = 0.0 + + #flipper controlls + self.__flipper_direction = int + self.__flipper_cmd_fr = Int8() + self.__flipper_cmd_fl = Int8() + self.__flipper_cmd_rr = Int8() + self.__flipper_cmd_rl = Int8() + + #chain controlls + self.__chain_msg = Twist() + self.__chain_msg.linear.y = float(0) + self.__chain_msg.linear.z = float(0) + self.__chain_msg.angular.y = float(0) + self.__chain_msg.angular.x = float(0) + + self.__direction = bool(False) + self.__direction_old = bool(True) + self.__button_space_pressed = bool(False) + self.__button_space_last_pressed = 0.0 + + #arm controlls + self.__arm_msg = TwistStamped() + self.__arm_msg.header.frame_id = "" + + #gripper controlls + self.__gripper_msg = JointJog() + self.__gripper_msg.joint_names = ["arm_link1_joint","arm_link2_joint", "arm_link3_joint", "arm_link4_joint", "arm_link5_joint"] + self.__gripper_msg.duration = 0.05 + + #Init class ->create subscriber, create timer + self.__readParams() + self.__callServo() + self.__createSubscribers() + self.__createPublishers() + self.__createTimer() + + self.get_logger().info("Tele_OP initiated") + + + + def __readParams(self): + #declare parameters + self.declare_parameter('Publish_rate', 20) #[Hz] + + #read parameters + self.__Publish_rate = rclpy.parameter.Parameter( + 'Publish_rate', + rclpy.Parameter.Type.DOUBLE, + 20.0 + ) + + + + #the servo_node service needs to be called to start + def __callServo(self): + self.__cli = self.create_client(Trigger, '/servo_node/start_servo') + while not self.__cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info('service not available, waiting again...') + self.__future = self.__cli.call_async(Trigger.Request()) + rclpy.spin_until_future_complete(self, self.__future) + return self.__future.result() + + + + def __checkCMDOutputEnable(self): + + # ----- Enable button check ----- + # If there was no joy msg received before and the twist output somehow is ENABLED, DISABLE the twist output. + if (self.__first_joy_msg_received == False and self.__joy_enabled != False): + self.__joy_enabled = False + + elif (self.__first_joy_msg_received == True): + # If the output is disabled, the output-enable button was not pressed and is now pressed, set the button + # pressed var to true and save the time point when it was pressed to get the time difference. + if(self.__joy_enabled == False and self.__button_opt_right_pressed == False and self.__button_opt_right == 1): + self.__button_opt_right_pressed = True + self.__button_opt_right_last_pressed = time.time() + + # check if button is pressed long enough. + elif (self.__joy_enabled == False and self.__button_opt_right_pressed == True and self.__button_opt_right == 1): + # messure how long the start button is pressed + time_difference = time.time() - self.__button_opt_right_last_pressed + + # If the start button is pressed long enough, enable the controlles + if(time_difference > 1): + self.__joy_enabled = True + + # After the output was enabled, wait for the button to be released befor the button_pressed var is set to false. + elif (self.__joy_enabled == True and self.__button_opt_right_pressed == True and self.__button_opt_right == 0): + self.__button_opt_right_pressed = False + + # if joy is disabled and the start_button was pressed but is released before the output could be + # enabled, the button_pressed var is set to false to receive a new button press. + elif (self.__joy_enabled == False and self.__button_opt_right_pressed == True and self.__button_opt_right == 0): + self.__button_opt_right_pressed = False + + #if joy is enabled and the enable button is pressed, disable the output. + elif (self.__joy_enabled == True and self.__button_opt_right_pressed == False and self.__button_opt_right == 1): + self.__joy_enabled = False + + + # Print a message to show the current output status. + if self.__joy_enabled != self.__joy_enabled_old: + if (self.__joy_enabled == True and self.__joy_enabled_old == False): + self.get_logger().info("Tele Op: ENABLED") + else: + if (self.__joy_enabled == False and self.__joy_enabled_old == True): + self.get_logger().info("Tele Op: DISABLED") + + # Set the old state to the new state + self.__joy_enabled_old = self.__joy_enabled + + + + def __checkArmMode(self): + + # ----- Enable button check ----- + # If there was no joy msg received before and the arm mode is ENABLED, DISABLE it. + if (self.__first_joy_msg_received == False and self.__arm_enabled != False): + self.__arm_enabled = False + + elif (self.__first_joy_msg_received == True): + #if current mode equals old mode and button is newly pressed, start timer + if (self.__arm_enabled != self.__arm_enabled_old and self.__button_opt_left == 1 and self.__button_opt_left_pressed == False): + self.__button_opt_left_pressed = True + self.__button_opt_left_last_pressed = time.time() + + #if timer has been started before + elif (self.__arm_enabled != self.__arm_enabled_old and self.__button_opt_left == 1 and self.__button_opt_left_pressed == True): + # messure how long the start button has been pressed + time_difference = time.time() - self.__button_opt_left_last_pressed + # If the start button has been pressed long enough, switch mode + if (time_difference > 1): + self.__arm_enabled = not self.__arm_enabled + if (self.__arm_enabled == True): + + self.get_logger().info("Control Mode: ARM") + else: + self.get_logger().info("Control Mode: BODY") + + #if mode has been switched, wait until start button is released, then change old mode status to enable new switch cycle + elif (self.__arm_enabled == self.__arm_enabled_old and self.__button_opt_left_pressed == True and self.__button_opt_left == 0): + self.__button_opt_left_pressed = False + self.__arm_enabled_old = not self.__arm_enabled_old + + # if button is not pressed long enough, reset + elif (self.__arm_enabled != self.__arm_enabled_old and self.__button_opt_left_pressed == True and self.__button_opt_left == 0): + self.__button_opt_left_pressed = False + + + + def __checkDir(self): + #if current mode equals old mode and button is newly pressed, start timer + if (self.__direction != self.__direction_old and self.__button_space == 1 and self.__button_space_pressed == False): + self.__button_space_pressed = True + self.__button_space_last_pressed = time.time() + + #if timer has been started before + elif (self.__direction != self.__direction_old and self.__button_space == 1 and self.__button_space_pressed == True): + # messure how long the start button has been pressed + time_difference = time.time() - self.__button_space_last_pressed + # If the start button has been pressed long enough, switch mode + if (time_difference > 1): + self.__direction = not self.__direction + if(self.__direction == False): + self.get_logger().info("Drive forward") + elif(self.__direction == True): + self.get_logger().info("Drive backward") + + #if mode has been switched, wait until start button is released, then change old mode status to enable new switch cycle + elif (self.__direction == self.__direction_old and self.__button_space_pressed == True and self.__button_space == 0): + self.__button_space_pressed = False + self.__direction_old = not self.__direction_old + + # if button is not pressed long enough, reset + elif (self.__direction != self.__direction_old and self.__button_space_pressed == True and self.__button_space == 0): + self.__button_space_pressed = False + + def __calcAndSendFlippers(self): + + if (self.__button_rb == 1 and self.__button_lb == 0): + self.__flipper_direction = 1 + elif(self.__button_rb == 0 and self.__button_lb == 1): + self.__flipper_direction = -1 + else: + self.__flipper_direction = 0 + + if(self.__arm_enabled == False): + self.__flipper_cmd_fr.data = self.__button_y * self.__flipper_direction + self.__flipper_cmd_fl.data = self.__button_x * self.__flipper_direction + self.__flipper_cmd_rr.data = self.__button_b * self.__flipper_direction + self.__flipper_cmd_rl.data = self.__button_a * self.__flipper_direction + else: + self.__flipper_cmd_fr.data = 0 + self.__flipper_cmd_fl.data = 0 + self.__flipper_cmd_rr.data = 0 + self.__flipper_cmd_rl.data = 0 + + + #send the commands + if (self.__joy_enabled == True): + self.__flipper_publisher_fr.publish(self.__flipper_cmd_fr) + self.__flipper_publisher_fl.publish(self.__flipper_cmd_fl) + self.__flipper_publisher_rr.publish(self.__flipper_cmd_rr) + self.__flipper_publisher_rl.publish(self.__flipper_cmd_rl) + + + + def __calcAndSendChains(self): + #check for arm mode and construct messages to be sent + if(self.__arm_enabled == False): + if(self.__direction == True): + self.__chain_msg.linear.x = -self.__left_stick_y + self.__chain_msg.angular.z = -self.__left_stick_x + else: + self.__chain_msg.linear.x = self.__left_stick_y + self.__chain_msg.angular.z = self.__left_stick_x + else: + self.__chain_msg.linear.x = 0.0 + self.__chain_msg.angular.z = 0.0 + + #send movement commands + if (self.__joy_enabled == True): + self.__chain_publisher.publish(self.__chain_msg) + + + + def __calc_and_send_arm(self): + + #calc arm tilt + tilt = abs((self.__right_trigger -1)/2) + (self.__left_trigger -1)/2 + + #check for arm mode + if(self.__arm_enabled == True): + #construct arm message + self.__arm_msg.twist.linear.x = self.__d_pad_x + self.__arm_msg.twist.linear.y = - self.__left_stick_y + self.__arm_msg.twist.linear.z = self.__d_pad_y + self.__arm_msg.twist.angular.z = self.__left_stick_x + self.__arm_msg.twist.angular.y = 0.0 + self.__arm_msg.twist.angular.x = tilt + + #construct joint message + joint1 = float(self.__button_rb -self.__button_lb) + joint2 = float(self.__button_x - self.__button_a) + joint3 = float(-self.__button_y + self.__button_b) + + self.__gripper_msg.velocities = [joint1, joint2, joint3, - self.__right_stick_y, - self.__right_stick_x] + + else: + self.__arm_msg.twist.linear.x = 0.0 + self.__arm_msg.twist.linear.y = 0.0 + self.__arm_msg.twist.linear.z = 0.0 + self.__arm_msg.twist.angular.z = 0.0 + self.__arm_msg.twist.angular.y = 0.0 + self.__arm_msg.twist.angular.x = 0.0 + + self.__gripper_msg.velocities = [0.0, 0.0, 0.0, 0.0, 0.0] + + #add time stamps and seq number + self.__gripper_msg.header.stamp = self.get_clock().now().to_msg() + self.__arm_msg.header.stamp = self.get_clock().now().to_msg() + + #send arm commands + if (self.__joy_enabled == True): + self.__arm_publisher.publish(self.__arm_msg) + self.__gripper_publisher.publish(self.__gripper_msg) + + + + def __timerCallback(self): + self.__checkCMDOutputEnable() + self.__checkArmMode() + self.__checkDir() + self.__calcAndSendFlippers() + self.__calcAndSendChains() + self.__calc_and_send_arm() + + + + def __joyCallback(self, msg): + #save controller input to local variables + + #axes + self.__left_stick_x_lock.acquire() + self.__left_stick_x = msg.axes[left_stick_x] + self.__left_stick_x_lock.release() + + self.__left_stick_y_lock.acquire() + self.__left_stick_y = msg.axes[left_stick_y] + self.__left_stick_y_lock.release() + + self.__left_trigger_lock.acquire() + self.__left_trigger = msg.axes[left_trigger] + self.__left_trigger_lock.release() + + self.__right_stick_x_lock.acquire() + self.__right_stick_x = msg.axes[right_stick_x] + self.__right_stick_x_lock.release() + + self.__right_stick_y_lock.acquire() + self.__right_stick_y = msg.axes[right_stick_y] + self.__right_stick_y_lock.release() + + self.__right_trigger_lock.acquire() + self.__right_trigger = msg.axes[right_trigger] + self.__right_trigger_lock.release() + + self.__d_pad_y_lock.acquire() + self.__d_pad_y = msg.axes[d_pad_y] + self.__d_pad_y_lock.release() + + self.__d_pad_x_lock.acquire() + self.__d_pad_x = msg.axes[d_pad_x] + self.__d_pad_x_lock.release() + + #buttons + self.__button_x_lock.acquire() + self.__button_x = msg.buttons[x_button] + self.__button_x_lock.release() + + self.__button_y_lock.acquire() + self.__button_y = msg.buttons[y_button] + self.__button_y_lock.release() + + self.__button_a_lock.acquire() + self.__button_a = msg.buttons[a_button] + self.__button_a_lock.release() + + self.__button_b_lock.acquire() + self.__button_b = msg.buttons[b_button] + self.__button_b_lock.release() + + self.__button_lb_lock.acquire() + self.__button_lb = msg.buttons[lb_button] + self.__button_lb_lock.release() + + self.__button_rb_lock.acquire() + self.__button_rb = msg.buttons[rb_button] + self.__button_rb_lock.release() + + self.__button_opt_right_lock.acquire() + self.__button_opt_right = msg.buttons[opt_right_button] + self.__button_opt_right_lock.release() + + self.__button_opt_left_lock.acquire() + self.__button_opt_left = msg.buttons[opt_left_button] + self.__button_opt_left_lock.release() + + self.__button_space_lock.acquire() + self.__button_space = msg.buttons[space_button] + self.__button_space_lock.release() + + + # Check if there was a joy msgs since the node was started + if(self.__first_joy_msg_received == False): + self.__first_joy_msg_received = True + + + def __createSubscribers(self): + # Create subscribers + + self._joy_sub = self.create_subscription( + Joy, + 'joy', + self.__joyCallback, + 5, + ) + + def __createPublishers(self): + # Create publishers + + self.__chain_publisher = self.create_publisher( + Twist, + 'cmd/drive', + 1 + ) + + self.__flipper_publisher_fr = self.create_publisher( + Int8, + 'cmd/flipper_fr', + 1 + ) + + self.__flipper_publisher_fl = self.create_publisher( + Int8, + 'cmd/flipper_fl', + 1 + ) + + self.__flipper_publisher_rr = self.create_publisher( + Int8, + 'cmd/flipper_rr', + 1 + ) + + self.__flipper_publisher_rl = self.create_publisher( + Int8, + 'cmd/flipper_rl', + 1 + ) + + self.__arm_publisher = self.create_publisher( + TwistStamped, + "cmd/arm/joy_twiststamped", + 1 + ) + + self.__gripper_publisher = self.create_publisher( + JointJog, + "cmd/arm/joy_joint", + 1 + ) + + def __createTimer(self): + # Create timer + self._timer = self.create_timer( + 1.0 / self.__Publish_rate.value, + self.__timerCallback + ) + + +def main(args=None): + rclpy.init(args=args) + + tele_op = TeleOp() + + rclpy.spin(tele_op) + + # Destroy the node explicitly + # (optional - otherwise it will be done automatically + # when the garbage collector destroys the node object) + tele_op.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/control_ws/src/lotti_teleop/package.xml b/control_ws/src/lotti_teleop/package.xml new file mode 100644 index 0000000..0d07072 --- /dev/null +++ b/control_ws/src/lotti_teleop/package.xml @@ -0,0 +1,30 @@ + + + + lotti_teleop + 0.0.0 + Xbox-based teleoperation node for the Lotti3 drive, flippers, and MoveIt Servo arm control. + paul + Apache-2.0 + + ament_python + + control_msgs + geometry_msgs + launch + launch_ros + rclpy + ros2launch + sensor_msgs + std_msgs + std_srvs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/chain_controller/resource/chain_controller b/control_ws/src/lotti_teleop/resource/lotti_teleop similarity index 100% rename from chain_controller/resource/chain_controller rename to control_ws/src/lotti_teleop/resource/lotti_teleop diff --git a/control_ws/src/lotti_teleop/setup.cfg b/control_ws/src/lotti_teleop/setup.cfg new file mode 100644 index 0000000..50fd966 --- /dev/null +++ b/control_ws/src/lotti_teleop/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lotti_teleop +[install] +install_scripts=$base/lib/lotti_teleop diff --git a/chain_controller/setup.py b/control_ws/src/lotti_teleop/setup.py similarity index 58% rename from chain_controller/setup.py rename to control_ws/src/lotti_teleop/setup.py index 47186db..fa3f216 100644 --- a/chain_controller/setup.py +++ b/control_ws/src/lotti_teleop/setup.py @@ -1,29 +1,30 @@ -from setuptools import find_packages, setup -import os from glob import glob +import os + +from setuptools import find_packages, setup -package_name = 'chain_controller' +package_name = 'lotti_teleop' setup( name=package_name, version='0.0.0', - packages=[package_name], + packages=find_packages(exclude=['test']), data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch/'), glob('launch/*launch.[pxy][yma]*')), + (os.path.join('share', package_name, 'launch/'), glob('launch/*launch.py')), ], install_requires=['setuptools'], zip_safe=True, - maintainer='resqbots', - maintainer_email='resqbots@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', + maintainer='paul', + maintainer_email='73643665+PaulKupka@users.noreply.github.com', + description='Xbox-based teleoperation node for the Lotti3 drive, flippers, and arm servo control.', + license='Apache-2.0', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'chain_controller = chain_controller.chain_controller:main' + 'teleop = lotti_teleop.lotti_teleop:main', ], }, ) diff --git a/chain_controller/test/test_copyright.py b/control_ws/src/lotti_teleop/test/test_copyright.py similarity index 100% rename from chain_controller/test/test_copyright.py rename to control_ws/src/lotti_teleop/test/test_copyright.py diff --git a/chain_controller/test/test_flake8.py b/control_ws/src/lotti_teleop/test/test_flake8.py similarity index 100% rename from chain_controller/test/test_flake8.py rename to control_ws/src/lotti_teleop/test/test_flake8.py diff --git a/chain_controller/test/test_pep257.py b/control_ws/src/lotti_teleop/test/test_pep257.py similarity index 100% rename from chain_controller/test/test_pep257.py rename to control_ws/src/lotti_teleop/test/test_pep257.py diff --git a/control_ws/src/lotti_vision/launch/multi_camera_launch.py b/control_ws/src/lotti_vision/launch/multi_camera_launch.py new file mode 100644 index 0000000..d10d2e0 --- /dev/null +++ b/control_ws/src/lotti_vision/launch/multi_camera_launch.py @@ -0,0 +1,47 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + # Define your ideal mapping setup + possible_cameras = [ + {'name': 'camera_1', 'device': '/dev/video0'}, + {'name': 'camera_2', 'device': '/dev/video4'}, + #{'name': 'camera_3', 'device': '/dev/video4'}, + {'name': 'camera_4', 'device': '/dev/video10'} + ] + + launch_nodes = [] + + # Only spin up a publisher if the device is actually plugged in! + for config in possible_cameras: + if os.path.exists(config['device']): + launch_nodes.append( + Node( + package='webcam_publisher', + executable='ffmpeg_camera_node', + name=f"publisher_{config['name']}", + output='screen', + parameters=[{ + 'device': config['device'], + 'camera_name': config['name'], + 'width': 640, + 'height': 480, + 'fps': 30 + }] + ) + ) + else: + print(f"--> [SKIPPED] {config['name']} skipped: Hardware target {config['device']} is not connected.") + + #launch the subscriber dashboard + launch_nodes.append( + Node( + package='lotti_vision', + executable='camera_dashboard', + name='camera_dashboard', + output='screen' + ) + ) + + return LaunchDescription(launch_nodes) \ No newline at end of file diff --git a/resqbot_drive_interface/resqbot_drive_interface/__init__.py b/control_ws/src/lotti_vision/lotti_vision/__init__.py similarity index 100% rename from resqbot_drive_interface/resqbot_drive_interface/__init__.py rename to control_ws/src/lotti_vision/lotti_vision/__init__.py diff --git a/control_ws/src/lotti_vision/lotti_vision/camera_dashboard.py b/control_ws/src/lotti_vision/lotti_vision/camera_dashboard.py new file mode 100644 index 0000000..51268e6 --- /dev/null +++ b/control_ws/src/lotti_vision/lotti_vision/camera_dashboard.py @@ -0,0 +1,180 @@ +import rclpy +from rclpy.node import Node +from rclpy.qos import qos_profile_sensor_data +from sensor_msgs.msg import CompressedImage +from cv_bridge import CvBridge +import cv2 +import threading +import time +import os +from ament_index_python.packages import get_package_share_directory + +try: + from ultralytics import YOLO +except ImportError: # pragma: no cover - depends on optional local install + YOLO = None + +class CameraDashboard(Node): + def __init__(self): + super().__init__('camera_dashboard') + self.bridge = CvBridge() + + + self.camera_config = [ + {'name': 'front_left', 'topic': '/camera_1/image_raw/compressed'}, + {'name': 'front_right', 'topic': '/camera_2/image_raw/compressed'}, + {'name': 'back_left', 'topic': '/camera_3/image_raw/compressed'}, + {'name': 'back_right', 'topic': '/camera_4/image_raw/compressed'}, + {'name': 'arm_camera', 'topic': '/camera_5/image_raw/compressed'} + ] + + self.ai_camera_name = 'arm_camera' + + # build runtime tracking tracking states from matrix array + self.frames = {config['name']: None for config in self.camera_config} + self.detections = {config['name']: "" for config in self.camera_config} + + self.ai_processed_frame = None + self.yolo_model = None + self.lock = threading.Lock() + + self._configure_optional_ai() + + # generate native ROS 2 subscriptions from config array + for config in self.camera_config: + cam_name = config['name'] + topic_str = config['topic'] + + # Using a default argument inside lambda handles scoping boundaries cleanly + self.create_subscription( + CompressedImage, + topic_str, + lambda msg, name=cam_name: self.image_callback(msg, name), + qos_profile_sensor_data + ) + self.get_logger().info(f"subscribed: '{cam_name}' -> Listening on {topic_str}") + + if self.yolo_model is None: + self.get_logger().info("Dashboard running in standard mode. Object detection offline.") + else: + self.get_logger().info(f"Dashboard running. AI Scanner targeted on: [{self.ai_camera_name}]") + self.ai_thread = threading.Thread(target=self.ai_processing_worker, daemon=True) + self.ai_thread.start() + + def _configure_optional_ai(self): + """Loads target network architectures from index share trees.""" + if YOLO is None: + self.detections[self.ai_camera_name] = "YOLO unavailable: package missing" + self.get_logger().warning("Optional dependency 'ultralytics' not found. AI features disabled.") + return + + package_share_directory = get_package_share_directory('lotti_vision') + model_path = os.path.join(package_share_directory, 'models', 'best.onnx') + + if not os.path.exists(model_path): + self.detections[self.ai_camera_name] = "YOLO unavailable: model missing" + self.get_logger().warning(f"No ONNX weight profile resolved at path target: {model_path}") + return + + try: + self.yolo_model = YOLO(model_path, task='detect') + self.detections[self.ai_camera_name] = "Scanning target workspace..." + except Exception as exc: + self.yolo_model = None + self.detections[self.ai_camera_name] = "YOLO offline: initialization error" + self.get_logger().error(f"Failed to isolate and instantiate target weight model: {exc}") + + def image_callback(self, msg, cam_name): + """Processes high-frequency payload packets while dropping old packets to preserve zero-latency bounds.""" + try: + # Latency gate check + now = self.get_clock().now() + msg_time = rclpy.time.Time.from_msg(msg.header.stamp) + latency = (now - msg_time).nanoseconds / 1e9 + + if latency > 0.1: # Drop anything delayed by over 100ms + return + + cv_image = self.bridge.compressed_imgmsg_to_cv2(msg, desired_encoding='bgr8') + with self.lock: + self.frames[cam_name] = cv_image + except Exception as e: + self.get_logger().error(f"Failed packet frame unpack for target [{cam_name}]: {e}") + + def ai_processing_worker(self): + """Isolated background pipeline handler. Targets the arm camera explicitly.""" + while rclpy.ok(): + time.sleep(0.05) # Constrain execution overhead to 20 FPS max + + if self.yolo_model is None: + continue + + target_frame = None + with self.lock: + if self.frames[self.ai_camera_name] is not None: + target_frame = self.frames[self.ai_camera_name].copy() + + if target_frame is not None: + # Execute edge analytics + results = self.yolo_model.predict(source=target_frame, conf=0.6, verbose=False) + annotated_frame = results[0].plot() + detected_count = len(results[0].boxes) + + # --- Placeholder area for QR code parsing loops later --- + # codes = qr_scanner_function(target_frame) + + if detected_count > 0: + status_text = f"WARNING: {detected_count} Hazard(s) Detected!" + else: + status_text = "Scanning workspace clear..." + + with self.lock: + self.detections[self.ai_camera_name] = status_text + self.ai_processed_frame = annotated_frame + + +def main(args=None): + rclpy.init(args=args) + dashboard_node = CameraDashboard() + + ros_thread = threading.Thread(target=rclpy.spin, args=(dashboard_node,), daemon=True) + ros_thread.start() + + try: + while rclpy.ok(): + time.sleep(0.016) # Cap structural cycle draws at 60 Hz + + with dashboard_node.lock: + # Create point copies of current operational buffers + display_frames = {k: v.copy() if v is not None else None for k, v in dashboard_node.frames.items()} + display_text = dashboard_node.detections.copy() + + # Splice in the processed frame overlay if available + if dashboard_node.ai_processed_frame is not None: + display_frames[dashboard_node.ai_camera_name] = dashboard_node.ai_processed_frame.copy() + + # Loop through active setups and paint the UI window frames + for cam_name, frame in display_frames.items(): + if frame is not None: + text = display_text.get(cam_name, "") + if text: + cv2.putText(frame, text, (10, 30), cv2.FONT_HERSHEY_SIMPLEX, + 0.7, (0, 0, 255) if "WARNING" in text else (0, 255, 0), 2, cv2.LINE_AA) + + # Renders a cleanly capitalized window header name based on dictionary keys + window_title = cam_name.replace('_', ' ').upper() + " FEED" + cv2.imshow(window_title, frame) + + key = cv2.waitKey(1) & 0xFF + if key == ord('q'): + break + + except KeyboardInterrupt: + pass + finally: + dashboard_node.destroy_node() + rclpy.shutdown() + cv2.destroyAllWindows() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/control_ws/src/lotti_vision/models/best.onnx b/control_ws/src/lotti_vision/models/best.onnx new file mode 100644 index 0000000..bf64b41 Binary files /dev/null and b/control_ws/src/lotti_vision/models/best.onnx differ diff --git "a/control_ws/src/lotti_vision/models/best.onnx\357\200\272Zone.Identifier" "b/control_ws/src/lotti_vision/models/best.onnx\357\200\272Zone.Identifier" new file mode 100644 index 0000000..d6c1ec6 Binary files /dev/null and "b/control_ws/src/lotti_vision/models/best.onnx\357\200\272Zone.Identifier" differ diff --git a/control_ws/src/lotti_vision/package.xml b/control_ws/src/lotti_vision/package.xml new file mode 100644 index 0000000..7c6a08d --- /dev/null +++ b/control_ws/src/lotti_vision/package.xml @@ -0,0 +1,27 @@ + + + + lotti_vision + 0.0.0 + Multi-camera dashboard and optional YOLO-based hazard detection for the Lotti3 operator station. + resqbots + Apache-2.0 + + ament_python + + ament_index_python + cv_bridge + python3-numpy + python3-opencv + rclpy + sensor_msgs + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/resqbot_drive_interface/resource/resqbot_drive_interface b/control_ws/src/lotti_vision/resource/lotti_vision similarity index 100% rename from resqbot_drive_interface/resource/resqbot_drive_interface rename to control_ws/src/lotti_vision/resource/lotti_vision diff --git a/control_ws/src/lotti_vision/setup.cfg b/control_ws/src/lotti_vision/setup.cfg new file mode 100644 index 0000000..2b5edb0 --- /dev/null +++ b/control_ws/src/lotti_vision/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/lotti_vision +[install] +install_scripts=$base/lib/lotti_vision diff --git a/control_ws/src/lotti_vision/setup.py b/control_ws/src/lotti_vision/setup.py new file mode 100644 index 0000000..833acac --- /dev/null +++ b/control_ws/src/lotti_vision/setup.py @@ -0,0 +1,32 @@ +import os +from glob import glob +from setuptools import find_packages, setup + +package_name = 'lotti_vision' + +setup( + name=package_name, + version='0.0.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + + # --- ADD THIS LINE TO REGISTER YOUR LAUNCH FILES --- + (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*launch.[pxy][yma]*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='robot', + maintainer_email='robot@todo.todo', + description='FFmpeg low-latency camera streaming package', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'ffmpeg_camera_node = lotti_vision.ffmpeg_camera_node:main', + 'camera_dashboard = lotti_vision.camera_dashboard:main', + ], + }, +) \ No newline at end of file diff --git a/resqbot_drive_interface/test/test_copyright.py b/control_ws/src/lotti_vision/test/test_copyright.py similarity index 100% rename from resqbot_drive_interface/test/test_copyright.py rename to control_ws/src/lotti_vision/test/test_copyright.py diff --git a/resqbot_drive_interface/test/test_flake8.py b/control_ws/src/lotti_vision/test/test_flake8.py similarity index 100% rename from resqbot_drive_interface/test/test_flake8.py rename to control_ws/src/lotti_vision/test/test_flake8.py diff --git a/resqbot_drive_interface/test/test_pep257.py b/control_ws/src/lotti_vision/test/test_pep257.py similarity index 100% rename from resqbot_drive_interface/test/test_pep257.py rename to control_ws/src/lotti_vision/test/test_pep257.py diff --git a/control_ws/src/webcam_publisher/package.xml b/control_ws/src/webcam_publisher/package.xml new file mode 100644 index 0000000..810e2d9 --- /dev/null +++ b/control_ws/src/webcam_publisher/package.xml @@ -0,0 +1,12 @@ + + + webcam_publisher + 0.1.0 + Low-latency webcam publisher via FFmpeg + you + MIT + rclpy + sensor_msgs + cv_bridge + ament_python + diff --git a/control_ws/src/webcam_publisher/setup.cfg b/control_ws/src/webcam_publisher/setup.cfg new file mode 100644 index 0000000..06337a3 --- /dev/null +++ b/control_ws/src/webcam_publisher/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/webcam_publisher +[install] +install_scripts=$base/lib/webcam_publisher diff --git a/control_ws/src/webcam_publisher/setup.py b/control_ws/src/webcam_publisher/setup.py new file mode 100644 index 0000000..3a7fe98 --- /dev/null +++ b/control_ws/src/webcam_publisher/setup.py @@ -0,0 +1,15 @@ +from setuptools import setup + +package_name = 'webcam_publisher' + +setup( + name=package_name, + version='0.1.0', + packages=[package_name], + install_requires=['setuptools'], + entry_points={ + 'console_scripts': [ + 'ffmpeg_camera_node = webcam_publisher.ffmpeg_camera_node:main', + ], + }, +) diff --git a/resqbot_flipper_interface/resqbot_flipper_interface/__init__.py b/control_ws/src/webcam_publisher/webcam_publisher/__init__.py similarity index 100% rename from resqbot_flipper_interface/resqbot_flipper_interface/__init__.py rename to control_ws/src/webcam_publisher/webcam_publisher/__init__.py diff --git a/control_ws/src/webcam_publisher/webcam_publisher/ffmpeg_camera_node.py b/control_ws/src/webcam_publisher/webcam_publisher/ffmpeg_camera_node.py new file mode 100644 index 0000000..44c533d --- /dev/null +++ b/control_ws/src/webcam_publisher/webcam_publisher/ffmpeg_camera_node.py @@ -0,0 +1,114 @@ +import subprocess +import threading +import rclpy +from rclpy.node import Node +from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy +from sensor_msgs.msg import CompressedImage + +class FFmpegCameraNode(Node): + def __init__(self): + super().__init__('ffmpeg_camera_node') + + # Declare dynamic hardware tracking parameters + self.declare_parameter('device', '/dev/video0') + self.declare_parameter('camera_name', 'camera_1') + self.declare_parameter('width', 640) + self.declare_parameter('height', 480) + self.declare_parameter('fps', 30) + + device = self.get_parameter('device').value + camera_name = self.get_parameter('camera_name').value + width = self.get_parameter('width').value + height = self.get_parameter('height').value + fps = self.get_parameter('fps').value + + qos = QoSProfile( + reliability=ReliabilityPolicy.BEST_EFFORT, + history=HistoryPolicy.KEEP_LAST, + depth=1 + ) + + # Dynamically maps the target topic name based on launch configuration input + topic_name = f'/{camera_name}/image_raw/compressed' + self.publisher_ = self.create_publisher(CompressedImage, topic_name, qos) + + ffmpeg_cmd = [ + 'ffmpeg', + '-fflags', 'nobuffer', + '-flags', 'low_delay', + '-probesize', '32', + '-analyzeduration', '0', + '-thread_queue_size', '512', + '-f', 'v4l2', + '-input_format', 'mjpeg', + '-framerate', str(fps), + '-video_size', f'{width}x{height}', + '-i', device, + '-err_detect', 'ignore_err', + '-vcodec', 'copy', + '-f', 'mjpeg', + 'pipe:1' + ] + + self.process = subprocess.Popen( + ffmpeg_cmd, + stdout=subprocess.PIPE, + stderr=subprocess.DEVNULL, + bufsize=65536 + ) + + self.thread = threading.Thread(target=self._capture_loop, daemon=True) + self.thread.start() + self.get_logger().info(f'Initialized {camera_name} on hardware {device} -> Publishing to {topic_name}') + + def _capture_loop(self): + buffer = bytearray() + chunk_size = 4096 + + while rclpy.ok(): + if self.process.poll() is not None: + self.get_logger().error('FFmpeg process terminated unexpectedly.') + break + + chunk = self.process.stdout.read(chunk_size) + if not chunk: + continue + + buffer.extend(chunk) + + while True: + soi = buffer.find(b'\xff\xd8') + eoi = buffer.find(b'\xff\xd9') + + if soi != -1 and eoi != -1 and eoi > soi: + jpeg_data = buffer[soi:eoi+2] + buffer = buffer[eoi+2:] + + msg = CompressedImage() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = 'camera_frame' + msg.format = 'jpeg' + msg.data = bytes(jpeg_data) + self.publisher_.publish(msg) + else: + if soi == -1 and len(buffer) > chunk_size * 2: + buffer.clear() + break + + def destroy_node(self): + self.process.terminate() + super().destroy_node() + +def main(args=None): + rclpy.init(args=args) + node = FFmpegCameraNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/resqbot_drive_interface/99-arduino-udev.rules b/resqbot_drive_interface/99-arduino-udev.rules deleted file mode 100644 index a73bb1a..0000000 --- a/resqbot_drive_interface/99-arduino-udev.rules +++ /dev/null @@ -1,17 +0,0 @@ -#the unshielded FTDI cables have difficulty running at full speed -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{product}=="TTL232R-3V3", ATTRS{idProduct}=="6001", GROUP="dialout", MODE="0666" - -#this is a cheap nano where the lazy people didn't program the serial -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{product}=="FT232R USB UART", ATTRS{idProduct}=="6001", GROUP="dialout", MODE="0666" - -#this is a decent nano -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{product}=="ARDUINO NANO", ATTRS{idProduct}=="6001", GROUP="dialout", MODE="0666" - -# Arduino Uno -SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0043", GROUP="dialout", MODE="0666" - -# dog hunter AG Arduino Uno Rev3 -SUBSYSTEM=="tty", ATTRS{idVendor}=="2a03", ATTRS{idProduct}=="0043", GROUP="dialout", MODE="0666" - -# Latest 2018 Arduino Nano's from arduino.cc -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001 Future Technology Devices International, Ltd FT232 USB-Serial (UART) IC", GROUP="dialout", MODE="0666" \ No newline at end of file diff --git a/resqbot_drive_interface/LICENSE b/resqbot_drive_interface/LICENSE deleted file mode 100644 index 439b333..0000000 --- a/resqbot_drive_interface/LICENSE +++ /dev/null @@ -1,30 +0,0 @@ - -BSD 3-Clause License - -Copyright (c) 2023, Franconian Open Robotics & ResqBots -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/resqbot_drive_interface/README.md b/resqbot_drive_interface/README.md deleted file mode 100644 index 5634496..0000000 --- a/resqbot_drive_interface/README.md +++ /dev/null @@ -1,104 +0,0 @@ -# ResQBot Drive Interface ROS2 <-> Arduino UNO - -# Overview - -This node establish a communication between a ros2 network and an arduino uno. - -# Install - -```bash -# Install python serial -sudo apt update -sudo apt install python3-serial - -# Copy repository to ros2_ws/src/ directory -mkdir -p ~/ros2_ws/src/ -cd ~/ros2_ws/src - -git clone git@github.com:ResQBots/resqbot_drive_interface.git - -# If SSH is not working -git clone https://github.com/ResQBots/resqbot_drive_interface.git - -# Install UDEV rules -# UDEV rules are necessary that you can access the serial port without an additonal command -cd ~/ros2_ws/src/resqbot_drive_interface -sudo cp 99-arduino-udev.rules /etc/udev/rules.d/ - -# Add user to group dialout -sudo usermod -aG dialout $USER - -# Restart system (only necessary after first installation) -sudo reboot - -# Build package -cd ~/ros2_ws -colcon build - -# Source workspace -source ~/ros2_ws/install/setup.bash - -# Run node (with default settings) -ros2 launch resqbot_drive_interface default_launch.py - -# If node is running publish a drive speed via ros topic pub command -# Set left speed to 100 rpm -ros2 topic pub /drive_interface/cmd/speed/left std_msgs/msg/Float32 "data: 100.0" - -# Set right speed to 100 rpm -ros2 topic pub /drive_interface/cmd/speed/right std_msgs/msg/Float32 "data: 100.0" -``` - -# Parameters - -The node can be configured with different parameters. You can simply modify them in the launch file located in the launch/ directory. - -*Important note: If you change the launch file you have to run colcon build to copy it from the project directory to your workspace!* - -|Parameter|Data-Type|Default-Value|Description| -|-|-|-|- -|update_rate_hz | Double | 10.0 | Update rate of the node in [Hz] | -|serial_timeout_sec | Double | 0.1 | Maximum time for arduino board to response before timeout and reconnect is triggered | -|serial_name | String | /dev/ttyACM0 | Serial name of the device | -|serial_baudrate | Int | 115200 | Baudrate of the arduino board | - -# Arduino Example Program - -The program is located in the directory 'arduino/': - -```C -// -// ARDUINO UNO Example Program -// -// Received the desired speed from the ros node via the serial line and -// stores it into the global variables. -// A response is returend for connection alive check. - -int16_t speed_left = 0; -int16_t speed_right = 0; - -void setup() { - // put your setup code here, to run once: - Serial.begin(115200); -} - -void loop() { - // Read string until \n is received - String rx_msg = Serial.readStringUntil('\n'); - - // Export data from string - sscanf(rx_msg.c_str(), "L%iR%i\n", &speed_left, &speed_right); - - // Respond to ros node - Serial.print("Set: Speed-Left: "); - Serial.print(speed_left); - Serial.print(" [rpm] Speed Right: "); - Serial.print(speed_right); - Serial.println(" [rpm]"); -} -``` - -# TODO - -- Add timeout detection for ROS messages and set speed automatically to 0 or stop drives -- Add timeout detection in arduino sketch to stop drives if connection is gone \ No newline at end of file diff --git a/resqbot_drive_interface/launch/default_launch.py b/resqbot_drive_interface/launch/default_launch.py deleted file mode 100644 index af60777..0000000 --- a/resqbot_drive_interface/launch/default_launch.py +++ /dev/null @@ -1,31 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, TextSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - - return LaunchDescription([ - Node( - package='resqbot_drive_interface', - namespace='', - executable='drive_interface', - name='drive_interface', - output="screen", - emulate_tty=True, - parameters=[{ - "update_rate_hz": 10.0, - "serial_timeout_sec": 0.1, - "serial_name": "/dev/ttyACM0", - "serial_baudrate": 115200, - }], - arguments=['--ros-args', '--log-level', 'info'] - ), - ]) \ No newline at end of file diff --git a/resqbot_drive_interface/package.xml b/resqbot_drive_interface/package.xml deleted file mode 100644 index 125a05f..0000000 --- a/resqbot_drive_interface/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - resqbot_drive_interface - 0.0.0 - TODO: Package description - resqbot - TODO: License declaration - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/resqbot_drive_interface/resqbot_drive_interface/__pycache__/__init__.cpython-310.pyc b/resqbot_drive_interface/resqbot_drive_interface/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index 2103592..0000000 Binary files a/resqbot_drive_interface/resqbot_drive_interface/__pycache__/__init__.cpython-310.pyc and /dev/null differ diff --git a/resqbot_drive_interface/resqbot_drive_interface/__pycache__/drive_interface.cpython-310.pyc b/resqbot_drive_interface/resqbot_drive_interface/__pycache__/drive_interface.cpython-310.pyc deleted file mode 100644 index c751202..0000000 Binary files a/resqbot_drive_interface/resqbot_drive_interface/__pycache__/drive_interface.cpython-310.pyc and /dev/null differ diff --git a/resqbot_drive_interface/resqbot_drive_interface/drive_interface.py b/resqbot_drive_interface/resqbot_drive_interface/drive_interface.py deleted file mode 100644 index 2fbe2fd..0000000 --- a/resqbot_drive_interface/resqbot_drive_interface/drive_interface.py +++ /dev/null @@ -1,207 +0,0 @@ -import rclpy -import serial - -from rclpy.node import Node - -from std_msgs.msg import Float32 -from rclpy import qos -from threading import Lock - -# Global Defines -------------------------------------------------------------- -DRIVE_INTERFACE_STS_INIT = 0 -DRIVE_INTERFACE_STS_RUN = 1 - -# Class ----------------------------------------------------------------------- -class ResqDriveInterface(Node): - # This class represents the drive interface node. - # It is a bridge between the ros network and the arduino drive controller - - def __init__(self): - # Entrypoint of the class (first called) - super().__init__('drive_interface') - - # Create private variables - self.__flushCountdown = 0 - - # Status see timer callback for description - self.__state = DRIVE_INTERFACE_STS_INIT - self.__cmd_vel = 0.0 - self.__cmd_vel_lock = Lock() - self.__cmd_angle = 0.0 - self.__cmd_angle_lock = Lock() - - - # Init class -> read parameters, create subscribers, create timer (for update loop) - self.__readParams() - self.__createSubscribers() - self.__createTimer() - - def __velCallback(self, msg): - - self.__cmd_vel_lock.acquire() - self.__cmd_vel = msg.data - self.__cmd_vel_lock.release() - - def __angleCallback(self, msg): - # Called when a message is received on the right wheel topic - #self.get_logger().info('Right wheel speed: %f' % msg.data) - - self.__cmd_angle_lock.acquire() - self.__cmd_angle = msg.data - self.__cmd_angle_lock.release() - - def __timerCallback(self): - # Called when the timer is triggered with the update rate specified in the parameters - #self.get_logger().info('Timer callback') - - # Statemachine to handle connect and reconnect to arduino uno - # if it is unplugged while node is running - - # Init state - # Try to connect to arduino uno, otherwise stay in init state - if self.__state == DRIVE_INTERFACE_STS_INIT: - try: - # Open serial connection - self.__serial = serial.Serial(self._serial_name.value, self._serial_baudrate.value, timeout=self._serial_timeout.value) - - # Flush old data from buffers - self.__serial.flush() - - # Transition to run state - self.__state = DRIVE_INTERFACE_STS_RUN - - # Log success - self.get_logger().info('Connected to arduino uno at %s' % self._serial_name.value) - except: - # Stay in init state - self.__state = DRIVE_INTERFACE_STS_INIT - - # Log error - self.get_logger().error('Could not connect to arduino uno at %s' % self._serial_name.value) - - - # Run state - # Transmit data to arduino uno - elif self.__state == DRIVE_INTERFACE_STS_RUN: - - # Get speeds thread safe - self.__cmd_vel_lock.acquire() - cmd_vel = int(self.__cmd_vel * 100) - self.__cmd_vel_lock.release() - - self.__cmd_angle_lock.acquire() - cmd_angle = int(self.__cmd_angle) - self.__cmd_angle_lock.release() - - # Create tx message - tx_msg = format("V{}A{}\n".format(int(cmd_vel), int(cmd_angle))); - - # Log message / uncomment for debugging - self.get_logger().info('TX: %s' % tx_msg) - - # Send message - try: - self.__serial.write(tx_msg.encode('utf-8')) - - except: - # Transition to init state - self.__state = DRIVE_INTERFACE_STS_INIT - self.__serial.close() - - # Log error - self.get_logger().error('Could not send data to arduino uno -> Transition to init state') - ''' - # Read message - try: - self.__serial.reset_input_buffer() - rx_msg = self.__serial.readline().decode('utf-8').rstrip() - - # Log message / uncomment for debugging - self.get_logger().info('RX: %s' % rx_msg) - - except: - # Transition to init state - self.__state = DRIVE_INTERFACE_STS_INIT - self.__serial.close() - - # Log error - self.get_logger().error('Could not read data from arduino uno -> Transition to init state') - ''' - - def __readParams(self): - # Declare parameters - self.declare_parameter('update_rate_hz', 10.0) - self.declare_parameter('serial_timeout_sec', 0.1) - self.declare_parameter('serial_name', '/dev/ttyACM0') - self.declare_parameter('serial_baudrate', 115200) - - # Read parameters - - self._update_rate_hz = rclpy.parameter.Parameter( - 'update_rate_hz', - rclpy.Parameter.Type.DOUBLE, - 10.0 - ) - - self._serial_timeout = rclpy.parameter.Parameter( - 'serial_timeout_sec', - rclpy.Parameter.Type.DOUBLE, - 0.1 - ) - - self._serial_name = rclpy.parameter.Parameter( - 'serial_name', - rclpy.Parameter.Type.STRING, - '/dev/ttyACM0' - ) - - self._serial_baudrate = rclpy.parameter.Parameter( - 'serial_baudrate', - rclpy.Parameter.Type.INTEGER, - 115200 - ) - - # Check for valid settings - if (1.0 / self._update_rate_hz.value) < self._serial_timeout.value: - self.get_logger().error('Serial timeout cannot be greater than the update rate of the task!') - raise Exception('Serial timeout cannot be greater than the update rate of the task!') - - def __createSubscribers(self): - # Create subscribers - - self._sub_vel = self.create_subscription( - Float32, - 'movement/velocity', - self.__velCallback, - 1, - ) - - self._sub_angle = self.create_subscription( - Float32, - 'movement/angle', - self.__angleCallback, - 1, - ) - - def __createTimer(self): - # Create timer - - self._timer = self.create_timer( - 1.0 / self._update_rate_hz.value, - self.__timerCallback - ) - -# Main ---------------------------------------------------------------------- - -def main(args=None): - rclpy.init(args=args) - - drive_interface = ResqDriveInterface() - - rclpy.spin(drive_interface) - - drive_interface.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/resqbot_drive_interface/setup.cfg b/resqbot_drive_interface/setup.cfg deleted file mode 100644 index 50569b8..0000000 --- a/resqbot_drive_interface/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/resqbot_drive_interface -[install] -install_scripts=$base/lib/resqbot_drive_interface diff --git a/resqbot_drive_interface/setup.py b/resqbot_drive_interface/setup.py deleted file mode 100644 index b548cac..0000000 --- a/resqbot_drive_interface/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from setuptools import setup -import os -from glob import glob - -package_name = 'resqbot_drive_interface' - -setup( - name=package_name, - version='0.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch/'), glob('launch/*launch.[pxy][yma]*')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='resqbot', - maintainer_email='resqbot@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'drive_interface = resqbot_drive_interface.drive_interface:main' - ], - }, -) diff --git a/resqbot_flipper_interface/99-arduino-udev.rules b/resqbot_flipper_interface/99-arduino-udev.rules deleted file mode 100644 index a73bb1a..0000000 --- a/resqbot_flipper_interface/99-arduino-udev.rules +++ /dev/null @@ -1,17 +0,0 @@ -#the unshielded FTDI cables have difficulty running at full speed -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{product}=="TTL232R-3V3", ATTRS{idProduct}=="6001", GROUP="dialout", MODE="0666" - -#this is a cheap nano where the lazy people didn't program the serial -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{product}=="FT232R USB UART", ATTRS{idProduct}=="6001", GROUP="dialout", MODE="0666" - -#this is a decent nano -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{product}=="ARDUINO NANO", ATTRS{idProduct}=="6001", GROUP="dialout", MODE="0666" - -# Arduino Uno -SUBSYSTEM=="tty", ATTRS{idVendor}=="2341", ATTRS{idProduct}=="0043", GROUP="dialout", MODE="0666" - -# dog hunter AG Arduino Uno Rev3 -SUBSYSTEM=="tty", ATTRS{idVendor}=="2a03", ATTRS{idProduct}=="0043", GROUP="dialout", MODE="0666" - -# Latest 2018 Arduino Nano's from arduino.cc -SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001 Future Technology Devices International, Ltd FT232 USB-Serial (UART) IC", GROUP="dialout", MODE="0666" \ No newline at end of file diff --git a/resqbot_flipper_interface/LICENSE b/resqbot_flipper_interface/LICENSE deleted file mode 100644 index 439b333..0000000 --- a/resqbot_flipper_interface/LICENSE +++ /dev/null @@ -1,30 +0,0 @@ - -BSD 3-Clause License - -Copyright (c) 2023, Franconian Open Robotics & ResqBots -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. \ No newline at end of file diff --git a/resqbot_flipper_interface/README.md b/resqbot_flipper_interface/README.md deleted file mode 100644 index 5634496..0000000 --- a/resqbot_flipper_interface/README.md +++ /dev/null @@ -1,104 +0,0 @@ -# ResQBot Drive Interface ROS2 <-> Arduino UNO - -# Overview - -This node establish a communication between a ros2 network and an arduino uno. - -# Install - -```bash -# Install python serial -sudo apt update -sudo apt install python3-serial - -# Copy repository to ros2_ws/src/ directory -mkdir -p ~/ros2_ws/src/ -cd ~/ros2_ws/src - -git clone git@github.com:ResQBots/resqbot_drive_interface.git - -# If SSH is not working -git clone https://github.com/ResQBots/resqbot_drive_interface.git - -# Install UDEV rules -# UDEV rules are necessary that you can access the serial port without an additonal command -cd ~/ros2_ws/src/resqbot_drive_interface -sudo cp 99-arduino-udev.rules /etc/udev/rules.d/ - -# Add user to group dialout -sudo usermod -aG dialout $USER - -# Restart system (only necessary after first installation) -sudo reboot - -# Build package -cd ~/ros2_ws -colcon build - -# Source workspace -source ~/ros2_ws/install/setup.bash - -# Run node (with default settings) -ros2 launch resqbot_drive_interface default_launch.py - -# If node is running publish a drive speed via ros topic pub command -# Set left speed to 100 rpm -ros2 topic pub /drive_interface/cmd/speed/left std_msgs/msg/Float32 "data: 100.0" - -# Set right speed to 100 rpm -ros2 topic pub /drive_interface/cmd/speed/right std_msgs/msg/Float32 "data: 100.0" -``` - -# Parameters - -The node can be configured with different parameters. You can simply modify them in the launch file located in the launch/ directory. - -*Important note: If you change the launch file you have to run colcon build to copy it from the project directory to your workspace!* - -|Parameter|Data-Type|Default-Value|Description| -|-|-|-|- -|update_rate_hz | Double | 10.0 | Update rate of the node in [Hz] | -|serial_timeout_sec | Double | 0.1 | Maximum time for arduino board to response before timeout and reconnect is triggered | -|serial_name | String | /dev/ttyACM0 | Serial name of the device | -|serial_baudrate | Int | 115200 | Baudrate of the arduino board | - -# Arduino Example Program - -The program is located in the directory 'arduino/': - -```C -// -// ARDUINO UNO Example Program -// -// Received the desired speed from the ros node via the serial line and -// stores it into the global variables. -// A response is returend for connection alive check. - -int16_t speed_left = 0; -int16_t speed_right = 0; - -void setup() { - // put your setup code here, to run once: - Serial.begin(115200); -} - -void loop() { - // Read string until \n is received - String rx_msg = Serial.readStringUntil('\n'); - - // Export data from string - sscanf(rx_msg.c_str(), "L%iR%i\n", &speed_left, &speed_right); - - // Respond to ros node - Serial.print("Set: Speed-Left: "); - Serial.print(speed_left); - Serial.print(" [rpm] Speed Right: "); - Serial.print(speed_right); - Serial.println(" [rpm]"); -} -``` - -# TODO - -- Add timeout detection for ROS messages and set speed automatically to 0 or stop drives -- Add timeout detection in arduino sketch to stop drives if connection is gone \ No newline at end of file diff --git a/resqbot_flipper_interface/launch/default_launch.py b/resqbot_flipper_interface/launch/default_launch.py deleted file mode 100644 index 229c9bd..0000000 --- a/resqbot_flipper_interface/launch/default_launch.py +++ /dev/null @@ -1,31 +0,0 @@ -import os - -from ament_index_python.packages import get_package_share_directory -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription -from launch.conditions import IfCondition -from launch.substitutions import LaunchConfiguration, TextSubstitution -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - - return LaunchDescription([ - Node( - package='resqbot_flipper_interface', - namespace='', - executable='flipper_interface', - name='flipper_interface', - output="screen", - emulate_tty=True, - parameters=[{ - "update_rate_hz": 10.0, - "serial_timeout_sec": 0.1, - "serial_name": "/dev/ttyACM1", - "serial_baudrate": 115200, - }], - arguments=['--ros-args', '--log-level', 'info'] - ), - ]) \ No newline at end of file diff --git a/resqbot_flipper_interface/package.xml b/resqbot_flipper_interface/package.xml deleted file mode 100644 index e7b226a..0000000 --- a/resqbot_flipper_interface/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - resqbot_flipper_interface - 0.0.0 - TODO: Package description - resqbot - TODO: License declaration - - - rclpy - serial - std_msgs - threading - os - setuptools - glob - - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/resqbot_flipper_interface/resource/resqbot_flipper_interface b/resqbot_flipper_interface/resource/resqbot_flipper_interface deleted file mode 100644 index e69de29..0000000 diff --git a/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/__init__.cpython-310.pyc b/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/__init__.cpython-310.pyc deleted file mode 100644 index 334a316..0000000 Binary files a/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/__init__.cpython-310.pyc and /dev/null differ diff --git a/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/drive_interface.cpython-310.pyc b/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/drive_interface.cpython-310.pyc deleted file mode 100644 index 2d2df3d..0000000 Binary files a/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/drive_interface.cpython-310.pyc and /dev/null differ diff --git a/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/flipper_interface.cpython-310.pyc b/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/flipper_interface.cpython-310.pyc deleted file mode 100644 index a9d5ec0..0000000 Binary files a/resqbot_flipper_interface/resqbot_flipper_interface/__pycache__/flipper_interface.cpython-310.pyc and /dev/null differ diff --git a/resqbot_flipper_interface/resqbot_flipper_interface/flipper_interface.py b/resqbot_flipper_interface/resqbot_flipper_interface/flipper_interface.py deleted file mode 100644 index 40ed123..0000000 --- a/resqbot_flipper_interface/resqbot_flipper_interface/flipper_interface.py +++ /dev/null @@ -1,253 +0,0 @@ -import rclpy -import serial - -from rclpy.node import Node - -from std_msgs.msg import Int16 -from rclpy import qos -from threading import Lock - -# Global Defines -------------------------------------------------------------- -FLIPPER_INTERFACE_STS_INIT = 0 -FLIPPER_INTERFACE_STS_RUN = 1 - -# Class ----------------------------------------------------------------------- -class ResqFlipperInterface(Node): - # This class represents the drive interface node. - # It is a bridge between the ros network and the arduino drive controller - - def __init__(self): - # Entrypoint of the class (first called) - super().__init__('flipper_interface') - - # Create private variables - # - - # Status see timer callback for description - self.__state = FLIPPER_INTERFACE_STS_INIT - self.__cmd_speed_frontLeft = 0.0 - self.__cmd_speed_frontLeft_lock = Lock() - self.__cmd_speed_frontRight = 0.0 - self.__cmd_speed_frontRight_lock = Lock() - self.__cmd_speed_rearLeft = 0.0 - self.__cmd_speed_rearLeft_lock = Lock() - self.__cmd_speed_rearRight = 0.0 - self.__cmd_speed_rearRight_lock = Lock() - self.__flushCountdown = 0 - - - # Init class -> read parameters, create subscribers, create timer (for update loop) - self.__readParams() - self.__createSubscribers() - self.__createTimer() - - def __flipper_frontLeft_callback(self, msg): - # Called when a message is received on the left wheel topic - #self.get_logger().info('front left Flipper mode: %f' % msg.data) - - self.__cmd_speed_frontLeft_lock.acquire() - self.__cmd_speed_frontLeft = msg.data - self.__cmd_speed_frontLeft_lock.release() - - def __flipper_frontRight_callback(self, msg): - # Called when a message is received on the left wheel topic - #self.get_logger().info('front right Flipper mode: %f' % msg.data) - - self.__cmd_speed_frontRight_lock.acquire() - self.__cmd_speed_frontRight = msg.data - self.__cmd_speed_frontRight_lock.release() - - def __flipper_rearLeft_callback(self, msg): - # Called when a message is received on the left wheel topic - #self.get_logger().info('rear left Flipper mode: %f' % msg.data) - - self.__cmd_speed_rearLeft_lock.acquire() - self.__cmd_speed_rearLeft = msg.data - self.__cmd_speed_rearLeft_lock.release() - - def __flipper_rearRight_callback(self, msg): - # Called when a message is received on the left wheel topic - #self.get_logger().info('rear right Flipper mode: %f' % msg.data) - - self.__cmd_speed_rearRight_lock.acquire() - self.__cmd_speed_rearRight = msg.data - self.__cmd_speed_rearRight_lock.release() - - def __timerCallback(self): - # Called when the timer is triggered with the update rate specified in the parameters - #self.get_logger().info('Timer callback') - - # Statemachine to handle connect and reconnect to arduino uno - # if it is unplugged while node is running - - # Init state - # Try to connect to arduino uno, otherwise stay in init state - if self.__state == FLIPPER_INTERFACE_STS_INIT: - try: - # Open serial connection - self.__serial = serial.Serial(self._serial_name.value, self._serial_baudrate.value, timeout=self._serial_timeout.value) - - # Flush old data from buffers - self.__serial.flush() - - # Transition to run state - self.__state = FLIPPER_INTERFACE_STS_RUN - - # Log success - self.get_logger().info('Connected to arduino uno at %s' % self._serial_name.value) - except: - # Stay in init state - self.__state = FLIPPER_INTERFACE_STS_INIT - - # Log error - self.get_logger().error('Could not connect to arduino uno at %s' % self._serial_name.value) - - - # Run state - # Transmit data to arduino uno - elif self.__state == FLIPPER_INTERFACE_STS_RUN: - - # Get speeds thread safe - self.__cmd_speed_frontLeft_lock.acquire() - cmd_speed_frontLeft = self.__cmd_speed_frontLeft - self.__cmd_speed_frontLeft_lock.release() - - self.__cmd_speed_frontRight_lock.acquire() - cmd_speed_frontRight = self.__cmd_speed_frontRight - self.__cmd_speed_frontRight_lock.release() - - self.__cmd_speed_rearLeft_lock.acquire() - cmd_speed_rearLeft = self.__cmd_speed_rearLeft - self.__cmd_speed_rearLeft_lock.release() - - self.__cmd_speed_rearRight_lock.acquire() - cmd_speed_rearRight = self.__cmd_speed_rearRight - self.__cmd_speed_rearRight_lock.release() - - # Create tx message - tx_msg = format("FL{}FR{}RL{}RR{}\n".format(int(cmd_speed_frontLeft), int(cmd_speed_frontRight), int(cmd_speed_rearLeft), int(cmd_speed_rearRight))); - - # Log message / uncomment for debugging - self.get_logger().info('TX: %s' % tx_msg) - - # Send message - try: - self.__serial.write(tx_msg.encode('utf-8')) - - except: - # Transition to init state - self.__state = FLIPPER_INTERFACE_STS_INIT - self.__serial.close() - - # Log error - self.get_logger().error('Could not send data to arduino uno -> Transition to init state') - - - # Read message -# try: -# self.__serial.reset_input_buffer() -# rx_msg = self.__serial.readline().decode('utf-8').rstrip() -# -# # Log message / uncomment for debugging -# #self.get_logger().info('RX: %s' % rx_msg) -# -# except: -# # Transition to init state -# self.__state = FLIPPER_INTERFACE_STS_INIT -# self.__serial.close() -# -# # Log error -# self.get_logger().error('Could not read data from arduino uno -> Transition to init state') - - - def __readParams(self): - # Declare parameters - self.declare_parameter('update_rate_hz', 10.0) - self.declare_parameter('serial_timeout_sec', 0.1) - self.declare_parameter('serial_name', '/dev/ttyACM1') - self.declare_parameter('serial_baudrate', 115200) - - # Read parameters - - self._update_rate_hz = rclpy.parameter.Parameter( - 'update_rate_hz', - rclpy.Parameter.Type.DOUBLE, - 10.0 - ) - - self._serial_timeout = rclpy.parameter.Parameter( - 'serial_timeout_sec', - rclpy.Parameter.Type.DOUBLE, - 0.1 - ) - - self._serial_name = rclpy.parameter.Parameter( - 'serial_name', - rclpy.Parameter.Type.STRING, - '/dev/ttyACM1' - ) - - self._serial_baudrate = rclpy.parameter.Parameter( - 'serial_baudrate', - rclpy.Parameter.Type.INTEGER, - 115200 - ) - - # Check for valid settings - if (1.0 / self._update_rate_hz.value) < self._serial_timeout.value: - self.get_logger().error('Serial timeout cannot be greater than the update rate of the task!') - raise Exception('Serial timeout cannot be greater than the update rate of the task!') - - def __createSubscribers(self): - # Create subscribers - - self._sub_flipper_frontLeft = self.create_subscription( - Int16, - 'cmd/flipper/frontLeft', - self.__flipper_frontLeft_callback, - 1, - ) - - self._sub_flipper_frontRight = self.create_subscription( - Int16, - 'cmd/flipper/frontRight', - self.__flipper_frontRight_callback, - 1, - ) - - self._sub_flipper_rearLeft = self.create_subscription( - Int16, - 'cmd/flipper/rearLeft', - self.__flipper_rearLeft_callback, - 1, - ) - - self._sub_flipper_rearRight = self.create_subscription( - Int16, - 'cmd/flipper/rearRight', - self.__flipper_rearRight_callback, - 1, - ) - - def __createTimer(self): - # Create timer - - self._timer = self.create_timer( - 1.0 / self._update_rate_hz.value, - self.__timerCallback - ) - -# Main ---------------------------------------------------------------------- - -def main(args=None): - rclpy.init(args=args) - - flipper_interface = ResqFlipperInterface() - - rclpy.spin(flipper_interface) - - flipper_interface.destroy_node() - rclpy.shutdown() - -if __name__ == '__main__': - main() diff --git a/resqbot_flipper_interface/setup.cfg b/resqbot_flipper_interface/setup.cfg deleted file mode 100644 index 3ac6990..0000000 --- a/resqbot_flipper_interface/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/resqbot_flipper_interface -[install] -install_scripts=$base/lib/resqbot_flipper_interface diff --git a/resqbot_flipper_interface/setup.py b/resqbot_flipper_interface/setup.py deleted file mode 100644 index d7207d9..0000000 --- a/resqbot_flipper_interface/setup.py +++ /dev/null @@ -1,29 +0,0 @@ -from setuptools import setup -import os -from glob import glob - -package_name = 'resqbot_flipper_interface' - -setup( - name=package_name, - version='0.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - (os.path.join('share', package_name, 'launch/'), glob('launch/*launch.[pxy][yma]*')), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='resqbot', - maintainer_email='resqbot@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'flipper_interface = resqbot_flipper_interface.flipper_interface:main' - ], - }, -) diff --git a/resqbot_flipper_interface/test/test_copyright.py b/resqbot_flipper_interface/test/test_copyright.py deleted file mode 100644 index 97a3919..0000000 --- a/resqbot_flipper_interface/test/test_copyright.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_copyright.main import main -import pytest - - -# Remove the `skip` decorator once the source file(s) have a copyright header -@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/resqbot_flipper_interface/test/test_flake8.py b/resqbot_flipper_interface/test/test_flake8.py deleted file mode 100644 index 27ee107..0000000 --- a/resqbot_flipper_interface/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/resqbot_flipper_interface/test/test_pep257.py b/resqbot_flipper_interface/test/test_pep257.py deleted file mode 100644 index b234a38..0000000 --- a/resqbot_flipper_interface/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/setup_jazzy.sh b/setup_jazzy.sh new file mode 100644 index 0000000..5931ff0 --- /dev/null +++ b/setup_jazzy.sh @@ -0,0 +1,404 @@ +#!/usr/bin/env bash +# ============================================================================= +# Res.Q Bots — Ubuntu 24.04 / ROS 2 Jazzy setup script +# +# Usage: +# chmod +x setup_jazzy.sh +# ./setup_jazzy.sh +# +# What this script does: +# 1. Configure locale +# 2. Install ROS 2 Jazzy (desktop) +# 3. Install Gazebo Harmonic + ROS-Gz bridge packages +# 4. Install all robot-specific ROS packages (MoveIt2, ros2_control, etc.) +# 5. Install system-level deps (libserial, OpenCV, ...) +# 6. Clone the resqbot_dev repo and build the workspace +# 7. Set up .bashrc (source ROS, set DOMAIN_ID) +# [Optional] Livox SDK2 + livox_ros_driver2 + Fast_LIO (LiDAR stack) +# [Optional] audio_common (robot audio) +# ============================================================================= + +set -euo pipefail +IFS=$'\n\t' + +# ── Colours ────────────────────────────────────────────────────────────────── +RED='\033[0;31m'; GREEN='\033[0;32m'; YELLOW='\033[1;33m' +BLUE='\033[0;34m'; BOLD='\033[1m'; NC='\033[0m' + +info() { echo -e "${BLUE}[INFO]${NC} $*"; } +ok() { echo -e "${GREEN}[OK]${NC} $*"; } +warn() { echo -e "${YELLOW}[WARN]${NC} $*"; } +header() { echo -e "\n${BOLD}${BLUE}══ $* ══${NC}"; } +die() { echo -e "${RED}[FAIL]${NC} $*" >&2; exit 1; } + +ask() { + # Returns 0 (yes) or 1 (no) + local prompt="$1" + local answer + read -rp "$(echo -e "${YELLOW}[?]${NC} ${prompt} [y/N] ")" answer + [[ "${answer,,}" == "y" || "${answer,,}" == "yes" ]] +} + +check_ubuntu_version() { + header "Checking OS" + local ver + ver=$(lsb_release -rs 2>/dev/null || echo "unknown") + if [[ "$ver" != "24.04" ]]; then + warn "This script targets Ubuntu 24.04 (detected: $ver)." + if ! ask "Continue anyway?"; then + die "Aborted." + fi + else + ok "Ubuntu 24.04 detected." + fi +} + +# ── Step 1: Locale ──────────────────────────────────────────────────────────── +setup_locale() { + header "Step 1 — Locale" + sudo apt update -q + sudo apt install -y locales + sudo locale-gen en_US en_US.UTF-8 + sudo update-locale LC_ALL=en_US.UTF-8 LANG=en_US.UTF-8 + export LANG=en_US.UTF-8 + ok "Locale configured." +} + +# ── Step 2: ROS 2 Jazzy ─────────────────────────────────────────────────────── +install_ros2_jazzy() { + header "Step 2 — ROS 2 Jazzy" + + if [[ -f /opt/ros/jazzy/setup.bash ]]; then + ok "ROS 2 Jazzy already installed — skipping." + return 0 + fi + + info "Adding ROS 2 apt repository..." + sudo apt install -y software-properties-common curl + sudo add-apt-repository -y universe + sudo curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ + -o /usr/share/keyrings/ros-archive-keyring.gpg + echo "deb [arch=$(dpkg --print-architecture) \ +signed-by=/usr/share/keyrings/ros-archive-keyring.gpg] \ +http://packages.ros.org/ros2/ubuntu \ +$(. /etc/os-release && echo "$UBUNTU_CODENAME") main" \ + | sudo tee /etc/apt/sources.list.d/ros2.list > /dev/null + + info "Installing ROS 2 Jazzy desktop + dev tools..." + sudo apt update -q + sudo apt install -y ros-dev-tools + sudo apt upgrade -y + sudo apt install -y ros-jazzy-desktop + + ok "ROS 2 Jazzy installed." +} + +# ── Step 3: Gazebo Harmonic + bridges ───────────────────────────────────────── +install_gazebo() { + header "Step 3 — Gazebo Harmonic + ROS-Gz packages" + + info "Installing Gazebo Harmonic and ROS bridges..." + sudo apt install -y \ + ros-jazzy-ros-gz-sim \ + ros-jazzy-gz-ros2-control \ + ros-jazzy-ros-gz-bridge \ + ros-jazzy-ros-gz-interfaces + + ok "Gazebo Harmonic packages installed." +} + +# ── Step 4: Robot-specific ROS packages ─────────────────────────────────────── +install_ros_packages() { + header "Step 4 — Robot ROS packages" + + info "Installing ros2_control, MoveIt2, controllers, and tools..." + sudo apt install -y \ + ros-jazzy-ros2-control \ + ros-jazzy-ros2-controllers \ + ros-jazzy-moveit \ + ros-jazzy-moveit-servo \ + ros-jazzy-joint-state-publisher-gui \ + ros-jazzy-xacro \ + ros-jazzy-robot-state-publisher \ + ros-jazzy-rviz2 \ + ros-jazzy-joy \ + ros-jazzy-cv-bridge \ + ros-jazzy-tf2-ros \ + ros-jazzy-tf2-tools \ + ros-jazzy-rqt-image-view + + info "Installing system libraries (serial, OpenCV, numpy, etc.)..." + sudo apt install -y \ + libserial-dev \ + python3-opencv \ + python3-numpy \ + python3-yaml + + info "Initialising rosdep..." + if [[ ! -f /etc/ros/rosdep/sources.list.d/20-default.list ]]; then + sudo rosdep init + else + ok "rosdep already initialised." + fi + rosdep update + + ok "ROS packages and system libs installed." +} + +# ── Step 5: Clone repo + build workspace ────────────────────────────────────── +setup_workspace() { + header "Step 5 — resqbot_dev workspace" + + local default_dir="$HOME/resqbot_dev" + local repo_dir + + read -rp "$(echo -e "${YELLOW}[?]${NC} Where to clone resqbot_dev? [${default_dir}] ")" repo_dir + repo_dir="${repo_dir:-$default_dir}" + + if [[ -d "$repo_dir/.git" ]]; then + ok "Repo already cloned at $repo_dir — pulling latest..." + git -C "$repo_dir" pull --rebase + else + info "Cloning resqbot_dev (branch: lotti3)..." + git clone --branch lotti3 \ + https://github.com/ResQBot/resqbot_dev.git \ + "$repo_dir" + fi + + local ws_dir="$repo_dir/control_ws" + info "Running rosdep in $ws_dir ..." + # Source ROS so rosdep can find Jazzy packages + # shellcheck disable=SC1091 + source /opt/ros/jazzy/setup.bash + rosdep install --from-paths "$ws_dir/src" --ignore-src -r -y + + info "Building workspace with colcon..." + cd "$ws_dir" + colcon build --symlink-install + + ok "Workspace built: $ws_dir" + echo "" + warn "NOTE: If the build fails because the Unitree SDK is missing, that is expected." + warn " Drive hardware needs to be built separately with:" + warn " colcon build --symlink-install --cmake-args -DLOTTI_ENABLE_VENDOR_UNITREE_SDK=ON -DUNITREE_SDK_DIR=/path/to/sdk" + echo "" + + # Store for bashrc step + export RESQBOT_WS="$ws_dir" +} + +# ── Step 6: .bashrc ─────────────────────────────────────────────────────────── +setup_bashrc() { + header "Step 6 — .bashrc" + + local bashrc="$HOME/.bashrc" + local ws="${RESQBOT_WS:-$HOME/resqbot_dev/control_ws}" + + append_if_missing() { + local line="$1" + if ! grep -qF "$line" "$bashrc"; then + echo "$line" >> "$bashrc" + info "Added: $line" + else + ok "Already present: $line" + fi + } + + append_if_missing "source /opt/ros/jazzy/setup.bash" + append_if_missing "source ${ws}/install/setup.bash" + append_if_missing "export ROS_DOMAIN_ID=17" + + ok ".bashrc updated." + info "Run 'source ~/.bashrc' or open a new terminal to activate." +} + +# ── Optional: Livox LiDAR stack ─────────────────────────────────────────────── +install_livox_stack() { + header "Optional — Livox SDK2 + livox_ros_driver2 + Fast_LIO" + + local livox_ws="$HOME/livox_ws" + mkdir -p "$livox_ws/src" + + # ── Livox SDK2 ── + info "Cloning Livox-SDK2..." + if [[ ! -d "$livox_ws/src/Livox-SDK2" ]]; then + git clone https://github.com/Livox-SDK/Livox-SDK2.git \ + "$livox_ws/src/Livox-SDK2" + fi + + info "Patching Livox-SDK2 for C++ std compliance..." + local files=( + "sdk_core/comm/define.h" + "sdk_core/logger_handler/file_manager.h" + ) + for f in "${files[@]}"; do + local fp="$livox_ws/src/Livox-SDK2/$f" + if ! grep -q "#include " "$fp"; then + sed -i '1s/^/#include \n/' "$fp" + info "Patched $f" + fi + done + + info "Building Livox-SDK2 (this takes a while)..." + mkdir -p "$livox_ws/src/Livox-SDK2/build" + cmake -S "$livox_ws/src/Livox-SDK2" -B "$livox_ws/src/Livox-SDK2/build" + make -j"$(nproc)" -C "$livox_ws/src/Livox-SDK2/build" + sudo make -C "$livox_ws/src/Livox-SDK2/build" install + ok "Livox-SDK2 installed." + + # ── livox_ros_driver2 ── + info "Cloning livox_ros_driver2..." + if [[ ! -d "$livox_ws/src/livox_ros_driver2" ]]; then + git clone https://github.com/Livox-SDK/livox_ros_driver2.git \ + "$livox_ws/src/livox_ros_driver2" + fi + + info "Building livox_ros_driver2 for Jazzy..." + # shellcheck disable=SC1091 + source /opt/ros/jazzy/setup.bash + cd "$livox_ws/src/livox_ros_driver2" + # The build script supports 'jazzy' directly + ./build.sh jazzy || { + warn "build.sh jazzy failed — trying with humble tag (common fallback)..." + ./build.sh humble + } + + append_if_missing_bashrc "source ${livox_ws}/install/setup.bash" + ok "livox_ros_driver2 built." + + # ── Fast_LIO ── + info "Installing Fast_LIO dependencies (PCL, Eigen3)..." + sudo apt install -y \ + pcl-tools \ + libpcl-dev \ + ros-jazzy-pcl-ros \ + libeigen3-dev + + local fast_lio_ws="$HOME/fast_lio_ws" + mkdir -p "$fast_lio_ws/src" + + info "Cloning Fast_LIO_ROS2..." + if [[ ! -d "$fast_lio_ws/src/FAST_LIO_ROS2" ]]; then + git clone --recursive \ + https://github.com/Ericsii/FAST_LIO_ROS2.git \ + "$fast_lio_ws/src/FAST_LIO_ROS2" + fi + + info "Patching Fast_LIO CMakeLists for C++20..." + sed -i 's/c++14/c++20/g; s/c++17/c++20/g' \ + "$fast_lio_ws/src/FAST_LIO_ROS2/CMakeLists.txt" + + # shellcheck disable=SC1091 + source /opt/ros/jazzy/setup.bash + source "$livox_ws/install/setup.bash" + rosdep install --from-paths "$fast_lio_ws/src" --ignore-src -y + + cd "$fast_lio_ws" + colcon build --symlink-install + + append_if_missing_bashrc "source ${fast_lio_ws}/install/setup.bash" + ok "Fast_LIO built." + warn "Edit ${fast_lio_ws}/src/FAST_LIO_ROS2/config/mid360.yaml to set your map_file_path." + warn "Edit the Livox MID360_config.json to match your host IP (see setup guide)." +} + +append_if_missing_bashrc() { + local line="$1" + if ! grep -qF "$line" "$HOME/.bashrc"; then + echo "$line" >> "$HOME/.bashrc" + fi +} + +# ── Optional: audio_common ──────────────────────────────────────────────────── +install_audio() { + header "Optional — audio_common" + + sudo apt install -y \ + libasound2-dev \ + gstreamer1.0-plugins-base \ + libgstreamer1.0-dev \ + liborc-0.4-dev \ + espeak \ + alsa-utils \ + pavucontrol + + local audio_ws="$HOME/ros2_audio_ws" + mkdir -p "$audio_ws/src" + + if [[ ! -d "$audio_ws/src/audio_common" ]]; then + git clone -b ros2 \ + https://github.com/ros-drivers/audio_common.git \ + "$audio_ws/src/audio_common" + fi + + # shellcheck disable=SC1091 + source /opt/ros/jazzy/setup.bash + rosdep install --from-paths "$audio_ws/src" --ignore-src -r -y + cd "$audio_ws" + colcon build --symlink-install + + append_if_missing_bashrc "source ${audio_ws}/install/setup.bash" + ok "audio_common built." +} + +# ── Summary ─────────────────────────────────────────────────────────────────── +print_summary() { + header "Setup complete" + echo "" + echo -e "${BOLD}What was installed:${NC}" + echo " ✓ ROS 2 Jazzy (desktop)" + echo " ✓ Gazebo Harmonic + ros_gz_sim + ros_gz_bridge + gz_ros2_control" + echo " ✓ MoveIt2 + moveit_servo" + echo " ✓ ros2_control + controllers" + echo " ✓ libserial-dev, OpenCV, NumPy" + echo " ✓ resqbot_dev workspace cloned + built" + echo " ✓ .bashrc configured (ROS source + DOMAIN_ID=17)" + echo "" + echo -e "${BOLD}Next steps:${NC}" + echo " 1. source ~/.bashrc (or open a new terminal)" + echo " 2. ros2 launch lotti_control3 gazebo.launch.py" + echo " 3. ros2 control list_controllers" + echo "" + echo -e "${BOLD}For real hardware (robot only):${NC}" + echo " - Drive: rebuild with -DLOTTI_ENABLE_VENDOR_UNITREE_SDK=ON" + echo " - Flipper: write_only mode until Arduino firmware sends feedback" + echo " - Both controllers are disabled by default in robot.launch.py:" + echo " ros2 launch lotti_control3 robot.launch.py enable_drive_controller:=true enable_flipper_controller:=true" + echo "" + warn "Simulation steps in CLAUDE.md are documentation-level until verified on this machine." +} + +# ── Main ────────────────────────────────────────────────────────────────────── +main() { + echo -e "${BOLD}" + echo "╔══════════════════════════════════════════════════╗" + echo "║ Res.Q Bots — Ubuntu 24.04 / ROS 2 Jazzy ║" + echo "║ Lotti3 full setup script ║" + echo "╚══════════════════════════════════════════════════╝" + echo -e "${NC}" + + check_ubuntu_version + setup_locale + install_ros2_jazzy + install_gazebo + install_ros_packages + setup_workspace + setup_bashrc + + echo "" + if ask "Install Livox LiDAR stack (SDK2 + livox_ros_driver2 + Fast_LIO)?"; then + install_livox_stack + else + info "Skipping Livox LiDAR stack." + fi + + if ask "Install audio_common (robot microphone / speaker)?"; then + install_audio + else + info "Skipping audio_common." + fi + + print_summary +} + +main "$@" diff --git a/web_video_server/AUTHORS.md b/web_video_server/AUTHORS.md deleted file mode 100644 index cf18e30..0000000 --- a/web_video_server/AUTHORS.md +++ /dev/null @@ -1,9 +0,0 @@ -Original Authors ----------------- - - * Mitchell Wills (mwills@wpi.edu) - -Contributors ------------- - - * [Russell Toris](http://users.wpi.edu/~rctoris/) (rctoris@wpi.edu) diff --git a/web_video_server/CHANGELOG.rst b/web_video_server/CHANGELOG.rst deleted file mode 100644 index 785d3a9..0000000 --- a/web_video_server/CHANGELOG.rst +++ /dev/null @@ -1,145 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package web_video_server -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.0.0 (2019-09-20) ------------------- -* Port to ROS 2 - -0.2.1 (2019-06-05) ------------------- -* Restream buffered frames with minimum publish rate (`#88 `_) - * Restream buffered frames with minimum publish rate - * Implement restreaming for ros_compressed_streamer -* Update travis config (`#89 `_) -* Fall back to mjpeg if ros_compressed is unavailable (`#87 `_) -* Contributors: Jihoon Lee, Viktor Kunovski, sfalexrog - -0.2.0 (2019-01-30) ------------------- -* Add "default_stream_type" parameter (`#84 `_) - This allows users to specify default stream type in their .launch files. Using a "ros_compressed" stream type sometimes - results in a much lower resource consumption, and having it set as a default is much nicer for end users. -* Add a workaround for MultipartStream constant busy state (`#83 `_) - * Add a workaround for MultipartStream constant busy state - * Remove C++11 features -* lax rule for topic name (`#77 `_) -* Add PngStreamer (`#74 `_) -* fix SteadyTimer check for backported ROS versions (`#71 `_) - i.e. on current kinetic -* Pkg format 2 (`#68 `_) - * use package format 2 - * add missing dependency on sensor_msgs -* fixed undeclared CODEC_FLAG_GLOBAL_HEADER (`#65 `_) -* Contributors: Andreas Klintberg, Dirk Thomas, Felix Ruess, Kazuto Murase, Viktor Kunovski, sfalexrog - -0.1.0 (2018-07-01) ------------------- -* Avoid queuing of images on slow ethernet connection (`#64 `_) -* use SteadyTimer (if available) for cleaning up inactive streams (`#63 `_) - * use SteadyTimer for cleaning up inactive streams - so that cleanup works correctly even if system time changes - SteadyTimer is available since roscpp 1.13.1 - * possibility to use SteadyTimer on older ROS versions - when SteadyTimer has been backported to those... -* Fix segfault in libav_streamer destructor (resolves `#59 `_) (`#60 `_) -* Fix vp8 in kinetic add vp9 and h264 support (`#52 `_) - * fix vp8 in kinetic - * add h264 and vp9 support -* Add Indigo test matrix in travis configuration (`#50 `_) -* Set image streamer as inactive if topic is not available (`#53 `_) - * Resolves `#38 `_ -* Fix Build for ubuntu 14.04 (`#48 `_) - * fix issue `#47 `_ - * fix double free -* Revert "use SteadyTimer for cleaning up inactive streams (`#45 `_)" (`#51 `_) - This reverts commit ae74f19ada22f288a7c7a99ada7a1b9b6037c7ce. -* use SteadyTimer for cleaning up inactive streams (`#45 `_) - so that cleanup works correctly even if system time changes -* Use trusty instead of xenial. See `travis-ci/travis-ci#7260 `_ (`#49 `_) - * Also see `RobotWebTools/rosbridge_suite#311 `_ -* Contributors: Felix Ruess, James Bailey, Jihoon Lee, randoms, schallerr - -0.0.7 (2017-11-20) ------------------- -* Ffmpeg 3 (`#43 `_) - * Correct use of deprecated parameters - codec_context\_->rc_buffer_aggressivity marked as "currently useless", so removed - codec_context\_->frame_skip_threshold access through new priv_data api - * New names for pixel formats - * AVPicture is deprecated, use AVFrame - * Switch to non-deprecated free functions - * Use new encoding api for newer versions - * codec_context is deprecated, use packet flags -* Update travis configuration to test against kinetic (`#44 `_) -* fixed misuse of remove_if (`#35 `_) -* Merge pull request `#33 `_ from achim-k/patch-1 - web_video_server: fix bool function not returning - This fix is required when compiling the package with `clang`. Otherwise a SIGILL (Illegal instruction) is triggered. -* Contributors: Hans-Joachim Krauch, Jan, Jihoon Lee, russelhowe - -0.0.6 (2017-01-17) ------------------- -* Fixed topic list to display all image topics, fixing Issue `#18 `_. -* Contributors: Eric - -0.0.5 (2016-10-13) ------------------- -* Merge pull request `#23 `_ from iki-wgt/develop - More information when server creation is failed -* Removed empty line -* More detailed exception message - Programm behavior is not changed since the exception is rethrown. -* Contributors: BennyRe, Russell Toris - -0.0.4 (2015-08-18) ------------------- -* Merge pull request #16 from mitchellwills/compressed - Adds support for streaming ROS compressed image topics without the need to reencode them -* Switch to checkout async_web_server_cpp from source -* Upgrade for change in signature of async_web_server_cpp request handler -* Added ros compressed video streamer type - This directly passes the ros compressed frame data to the http socket without reencoding it -* Switched from passing image transport to passing node handle to streamer constructors -* Added default transport parameter for regular image streamers -* Contributors: Mitchell Wills, Russell Toris - -0.0.3 (2015-05-07) ------------------- -* added verbose flag -* Contributors: Russell Toris - -0.0.2 (2015-02-20) ------------------- -* Merge pull request #10 from mitchellwills/develop - Added option to specify server address -* Added option to specify server address -* Merge pull request #3 from mitchellwills/develop - Remove old http_server implementation and replace it with async_web_server_cpp package -* Moved from using built in http server to new async_web_server_cpp package -* Did some cleanup of streamers -* Update package.xml -* Contributors: Mitchell Wills, Russell Toris - -0.0.1 (2014-10-30) ------------------- -* missed travis file -* cleanup and travis build -* ROS auto-format -* Merge pull request #1 from mitchellwills/develop - Initial implementation of a http web server that serves ROS image topics as multiple web compatible formats -* Made some changes suggested by catkin_lint and did some package cleanup -* Added support for libavstreamer on Ubuntu 13.10 version of libav -* Added support for specifying vp8 quality parameter -* Implemented lazy initialization for libav buffers so that output size can be infered from the first image -* Updated README -* Added vp8 support -* Broke image encodings out into different files -* Made write operations async - Send timestamps for mjpeg stream -* Initial commit -* Update README.md -* Update README.md -* Update README.md -* Initial commit -* Contributors: Mitchell Wills, Russell Toris diff --git a/web_video_server/CMakeLists.txt b/web_video_server/CMakeLists.txt deleted file mode 100644 index ce84c17..0000000 --- a/web_video_server/CMakeLists.txt +++ /dev/null @@ -1,83 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(web_video_server) - -find_package(ament_cmake_ros REQUIRED) - -find_package(async_web_server_cpp REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(image_transport REQUIRED) -find_package(rclcpp REQUIRED) -find_package(sensor_msgs REQUIRED) - -find_package(OpenCV REQUIRED) -find_package(Boost REQUIRED COMPONENTS thread) - -find_package(PkgConfig REQUIRED) -pkg_check_modules(avcodec libavcodec REQUIRED) -pkg_check_modules(avformat libavformat REQUIRED) -pkg_check_modules(avutil libavutil REQUIRED) -pkg_check_modules(swscale libswscale REQUIRED) - -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -################################################### -## Declare things to be passed to other projects ## -################################################### - -########### -## Build ## -########### - -## Specify additional locations of header files -include_directories(include - ${Boost_INCLUDE_DIRS} - ${avcodec_INCLUDE_DIRS} - ${avformat_INCLUDE_DIRS} - ${avutil_INCLUDE_DIRS} - ${swscale_INCLUDE_DIRS} -) - -## Declare a cpp executable -add_executable(${PROJECT_NAME} - src/web_video_server.cpp - src/image_streamer.cpp - src/libav_streamer.cpp - src/vp8_streamer.cpp - src/h264_streamer.cpp - src/vp9_streamer.cpp - src/multipart_stream.cpp - src/ros_compressed_streamer.cpp - src/jpeg_streamers.cpp - src/png_streamers.cpp -) - -ament_target_dependencies(${PROJECT_NAME} - async_web_server_cpp cv_bridge image_transport rclcpp sensor_msgs) - -## Specify libraries to link a library or executable target against -target_link_libraries(${PROJECT_NAME} - ${Boost_LIBRARIES} - ${OpenCV_LIBS} - ${avcodec_LIBRARIES} - ${avformat_LIBRARIES} - ${avutil_LIBRARIES} - ${swscale_LIBRARIES} -) - -############# -## Install ## -############# - -## Mark executables and/or libraries for installation -install(TARGETS ${PROJECT_NAME} - DESTINATION lib/${PROJECT_NAME} -) - -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include/${PROJECT_NAME} - FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" -) - -ament_package() diff --git a/web_video_server/LICENSE b/web_video_server/LICENSE deleted file mode 100644 index 872a63f..0000000 --- a/web_video_server/LICENSE +++ /dev/null @@ -1,32 +0,0 @@ -Software License Agreement (BSD License) - -Copyright (c) 2014, Worcester Polytechnic Institute -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions -are met: - - * Redistributions of source code must retain the above copyright - notice, this list of conditions and the following disclaimer. - * Redistributions in binary form must reproduce the above - copyright notice, this list of conditions and the following - disclaimer in the documentation and/or other materials provided - with the distribution. - * Neither the name of Worcester Polytechnic Institute - nor the names of its contributors may be used to - endorse or promote products derived from this software without - specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -POSSIBILITY OF SUCH DAMAGE. diff --git a/web_video_server/README.md b/web_video_server/README.md deleted file mode 100644 index cbe4568..0000000 --- a/web_video_server/README.md +++ /dev/null @@ -1,17 +0,0 @@ -web_video_server [![Build Status](https://api.travis-ci.org/RobotWebTools/web_video_server.png)](https://travis-ci.org/RobotWebTools/web_video_server) -================ - -#### HTTP Streaming of ROS Image Topics in Multiple Formats -This node combines the capabilities of [ros_web_video](https://github.com/RobotWebTools/ros_web_video) and [mjpeg_server](https://github.com/RobotWebTools/mjpeg_server) into a single node. - -For full documentation, see [the ROS wiki](http://ros.org/wiki/web_video_server). - -[Doxygen](http://docs.ros.org/indigo/api/web_video_server/html/) files can be found on the ROS wiki. - -This project is released as part of the [Robot Web Tools](http://robotwebtools.org/) effort. - -### License -web_video_server is released with a BSD license. For full terms and conditions, see the [LICENSE](LICENSE) file. - -### Authors -See the [AUTHORS](AUTHORS.md) file for a full list of contributors. diff --git a/web_video_server/cam1.yaml b/web_video_server/cam1.yaml deleted file mode 100644 index 1dee962..0000000 --- a/web_video_server/cam1.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - video_device: "/dev/video0" - framerate: 10.0 - io_method: "mmap" - frame_id: "cam1" - pixel_format: "yuyv2rgb" - color_format: "yuyv2rgb" - image_width: 320 - image_height: 240 - camera_name: "cam1" diff --git a/web_video_server/include/web_video_server/h264_streamer.h b/web_video_server/include/web_video_server/h264_streamer.h deleted file mode 100644 index 9961389..0000000 --- a/web_video_server/include/web_video_server/h264_streamer.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef H264_STREAMERS_H_ -#define H264_STREAMERS_H_ - -#include -#include "web_video_server/libav_streamer.h" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" - -namespace web_video_server -{ - -class H264Streamer : public LibavStreamer -{ -public: - H264Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - ~H264Streamer(); -protected: - virtual void initializeEncoder(); - std::string preset_; -}; - -class H264StreamerType : public LibavStreamerType -{ -public: - H264StreamerType(); - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); -}; - -} - -#endif - diff --git a/web_video_server/include/web_video_server/image_streamer.h b/web_video_server/include/web_video_server/image_streamer.h deleted file mode 100644 index 59a1a92..0000000 --- a/web_video_server/include/web_video_server/image_streamer.h +++ /dev/null @@ -1,93 +0,0 @@ -#ifndef IMAGE_STREAMER_H_ -#define IMAGE_STREAMER_H_ - -#include -#include -#include -#include -#include "async_web_server_cpp/http_server.hpp" -#include "async_web_server_cpp/http_request.hpp" - -namespace web_video_server -{ - -class ImageStreamer -{ -public: - ImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - - virtual void start() = 0; - virtual ~ImageStreamer(); - - bool isInactive() - { - return inactive_; - } - ; - - /** - * Restreams the last received image frame if older than max_age. - */ - virtual void restreamFrame(double max_age) = 0; - - std::string getTopic() - { - return topic_; - } - ; -protected: - async_web_server_cpp::HttpConnectionPtr connection_; - async_web_server_cpp::HttpRequest request_; - rclcpp::Node::SharedPtr nh_; - bool inactive_; - image_transport::Subscriber image_sub_; - std::string topic_; -}; - - -class ImageTransportImageStreamer : public ImageStreamer -{ -public: - ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - virtual ~ImageTransportImageStreamer(); - - virtual void start(); - -protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time) = 0; - virtual void restreamFrame(double max_age); - virtual void initialize(const cv::Mat &); - - image_transport::Subscriber image_sub_; - int output_width_; - int output_height_; - bool invert_; - std::string default_transport_; - - rclcpp::Time last_frame; - cv::Mat output_size_image; - boost::mutex send_mutex_; - -private: - image_transport::ImageTransport it_; - bool initialized_; - - void imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg); -}; - -class ImageStreamerType -{ -public: - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) = 0; - - virtual std::string create_viewer(const async_web_server_cpp::HttpRequest &request) = 0; -}; - -} - -#endif diff --git a/web_video_server/include/web_video_server/jpeg_streamers.h b/web_video_server/include/web_video_server/jpeg_streamers.h deleted file mode 100644 index ad788fa..0000000 --- a/web_video_server/include/web_video_server/jpeg_streamers.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef JPEG_STREAMERS_H_ -#define JPEG_STREAMERS_H_ - -#include -#include "web_video_server/image_streamer.h" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" -#include "web_video_server/multipart_stream.h" - -namespace web_video_server -{ - -class MjpegStreamer : public ImageTransportImageStreamer -{ -public: - MjpegStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - ~MjpegStreamer(); -protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); - -private: - MultipartStream stream_; - int quality_; -}; - -class MjpegStreamerType : public ImageStreamerType -{ -public: - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); -}; - -class JpegSnapshotStreamer : public ImageTransportImageStreamer -{ -public: - JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh); - ~JpegSnapshotStreamer(); -protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); - -private: - int quality_; -}; - -} - -#endif diff --git a/web_video_server/include/web_video_server/libav_streamer.h b/web_video_server/include/web_video_server/libav_streamer.h deleted file mode 100644 index 643e3b3..0000000 --- a/web_video_server/include/web_video_server/libav_streamer.h +++ /dev/null @@ -1,81 +0,0 @@ -#ifndef LIBAV_STREAMERS_H_ -#define LIBAV_STREAMERS_H_ - -#include -#include "web_video_server/image_streamer.h" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" - -extern "C" -{ -#include -#include -#include -#include -#include -#include -#include -#include -} - -namespace web_video_server -{ - -class LibavStreamer : public ImageTransportImageStreamer -{ -public: - LibavStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh, const std::string &format_name, const std::string &codec_name, - const std::string &content_type); - - ~LibavStreamer(); - -protected: - virtual void initializeEncoder(); - virtual void sendImage(const cv::Mat&, const rclcpp::Time& time); - virtual void initialize(const cv::Mat&); - AVOutputFormat* output_format_; - AVFormatContext* format_context_; - AVCodec* codec_; - AVCodecContext* codec_context_; - AVStream* video_stream_; - - AVDictionary* opt_; // container format options - -private: - AVFrame* frame_; - struct SwsContext* sws_context_; - rclcpp::Time first_image_timestamp_; - boost::mutex encode_mutex_; - - std::string format_name_; - std::string codec_name_; - std::string content_type_; - int bitrate_; - int qmin_; - int qmax_; - int gop_; - - uint8_t* io_buffer_; // custom IO buffer -}; - -class LibavStreamerType : public ImageStreamerType -{ -public: - LibavStreamerType(const std::string &format_name, const std::string &codec_name, const std::string &content_type); - - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); - -private: - const std::string format_name_; - const std::string codec_name_; - const std::string content_type_; -}; - -} - -#endif diff --git a/web_video_server/include/web_video_server/multipart_stream.h b/web_video_server/include/web_video_server/multipart_stream.h deleted file mode 100644 index d6ea359..0000000 --- a/web_video_server/include/web_video_server/multipart_stream.h +++ /dev/null @@ -1,44 +0,0 @@ -#ifndef MULTIPART_STREAM_H_ -#define MULTIPART_STREAM_H_ - -#include -#include - -#include - -namespace web_video_server -{ - -struct PendingFooter { - rclcpp::Time timestamp; - std::weak_ptr contents; -}; - -class MultipartStream { -public: - MultipartStream(std::function get_now, - async_web_server_cpp::HttpConnectionPtr& connection, - const std::string& boundry="boundarydonotcross", - std::size_t max_queue_size=1); - - void sendInitialHeader(); - void sendPartHeader(const rclcpp::Time &time, const std::string& type, size_t payload_size); - void sendPartFooter(const rclcpp::Time &time); - void sendPartAndClear(const rclcpp::Time &time, const std::string& type, std::vector &data); - void sendPart(const rclcpp::Time &time, const std::string& type, const boost::asio::const_buffer &buffer, - async_web_server_cpp::HttpConnection::ResourcePtr resource); - -private: - bool isBusy(); - -private: - std::function get_now_; - const std::size_t max_queue_size_; - async_web_server_cpp::HttpConnectionPtr connection_; - std::string boundry_; - std::queue pending_footers_; -}; - -} - -#endif diff --git a/web_video_server/include/web_video_server/png_streamers.h b/web_video_server/include/web_video_server/png_streamers.h deleted file mode 100644 index a6edabc..0000000 --- a/web_video_server/include/web_video_server/png_streamers.h +++ /dev/null @@ -1,51 +0,0 @@ -#ifndef PNG_STREAMERS_H_ -#define PNG_STREAMERS_H_ - -#include -#include "web_video_server/image_streamer.h" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" -#include "web_video_server/multipart_stream.h" - -namespace web_video_server -{ - -class PngStreamer : public ImageTransportImageStreamer -{ -public: - PngStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - ~PngStreamer(); -protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); - -private: - MultipartStream stream_; - int quality_; -}; - -class PngStreamerType : public ImageStreamerType -{ -public: - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); -}; - -class PngSnapshotStreamer : public ImageTransportImageStreamer -{ -public: - PngSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh); - ~PngSnapshotStreamer(); -protected: - virtual void sendImage(const cv::Mat &, const rclcpp::Time &time); - -private: - int quality_; -}; - -} - -#endif diff --git a/web_video_server/include/web_video_server/ros_compressed_streamer.h b/web_video_server/include/web_video_server/ros_compressed_streamer.h deleted file mode 100644 index 7219dfd..0000000 --- a/web_video_server/include/web_video_server/ros_compressed_streamer.h +++ /dev/null @@ -1,45 +0,0 @@ -#ifndef ROS_COMPRESSED_STREAMERS_H_ -#define ROS_COMPRESSED_STREAMERS_H_ - -#include -#include "web_video_server/image_streamer.h" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" -#include "web_video_server/multipart_stream.h" - -namespace web_video_server -{ - -class RosCompressedStreamer : public ImageStreamer -{ -public: - RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - ~RosCompressedStreamer(); - virtual void start(); - virtual void restreamFrame(double max_age); - -protected: - virtual void sendImage(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, const rclcpp::Time &time); - -private: - void imageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg); - MultipartStream stream_; - rclcpp::Subscription::SharedPtr image_sub_; - rclcpp::Time last_frame; - sensor_msgs::msg::CompressedImage::ConstSharedPtr last_msg; - boost::mutex send_mutex_; -}; - -class RosCompressedStreamerType : public ImageStreamerType -{ -public: - boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - std::string create_viewer(const async_web_server_cpp::HttpRequest &request); -}; - -} - -#endif diff --git a/web_video_server/include/web_video_server/vp8_streamer.h b/web_video_server/include/web_video_server/vp8_streamer.h deleted file mode 100644 index 46e8bed..0000000 --- a/web_video_server/include/web_video_server/vp8_streamer.h +++ /dev/null @@ -1,72 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2014, Worcester Polytechnic Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Worcester Polytechnic Institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - *********************************************************************/ - -#ifndef VP8_STREAMERS_H_ -#define VP8_STREAMERS_H_ - -#include -#include "web_video_server/libav_streamer.h" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" - -namespace web_video_server -{ - -class Vp8Streamer : public LibavStreamer -{ -public: - Vp8Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - ~Vp8Streamer(); -protected: - virtual void initializeEncoder(); -private: - std::string quality_; -}; - -class Vp8StreamerType : public LibavStreamerType -{ -public: - Vp8StreamerType(); - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); -}; - -} - -#endif - diff --git a/web_video_server/include/web_video_server/vp9_streamer.h b/web_video_server/include/web_video_server/vp9_streamer.h deleted file mode 100644 index 06c48f8..0000000 --- a/web_video_server/include/web_video_server/vp9_streamer.h +++ /dev/null @@ -1,33 +0,0 @@ -#ifndef VP9_STREAMERS_H_ -#define VP9_STREAMERS_H_ - -#include -#include "web_video_server/libav_streamer.h" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" - -namespace web_video_server -{ - -class Vp9Streamer : public LibavStreamer -{ -public: - Vp9Streamer(const async_web_server_cpp::HttpRequest& request, async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); - ~Vp9Streamer(); -protected: - virtual void initializeEncoder(); -}; - -class Vp9StreamerType : public LibavStreamerType -{ -public: - Vp9StreamerType(); - virtual boost::shared_ptr create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh); -}; - -} - -#endif diff --git a/web_video_server/include/web_video_server/web_video_server.h b/web_video_server/include/web_video_server/web_video_server.h deleted file mode 100644 index 1da6015..0000000 --- a/web_video_server/include/web_video_server/web_video_server.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef WEB_VIDEO_SERVER_H_ -#define WEB_VIDEO_SERVER_H_ - -#include -#include -#include -#include "web_video_server/image_streamer.h" -#include "async_web_server_cpp/http_server.hpp" -#include "async_web_server_cpp/http_request.hpp" -#include "async_web_server_cpp/http_connection.hpp" - -namespace web_video_server -{ - -/** - * @class WebVideoServer - * @brief - */ -class WebVideoServer -{ -public: - /** - * @brief Constructor - * @return - */ - WebVideoServer(rclcpp::Node::SharedPtr &nh, rclcpp::Node::SharedPtr &private_nh); - - /** - * @brief Destructor - Cleans up - */ - virtual ~WebVideoServer(); - - /** - * @brief Starts the server and spins - */ - void spin(); - - void setup_cleanup_inactive_streams(); - - bool handle_stream(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); - - bool handle_stream_viewer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); - - bool handle_snapshot(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); - - bool handle_list_streams(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, const char* end); - -private: - void restreamFrames(double max_age); - void cleanup_inactive_streams(); - - rclcpp::Node::SharedPtr nh_; - rclcpp::WallTimer::SharedPtr cleanup_timer_; - int ros_threads_; - double publish_rate_; - int port_; - std::string address_; - boost::shared_ptr server_; - async_web_server_cpp::HttpRequestHandlerGroup handler_group_; - - std::vector > image_subscribers_; - std::map > stream_types_; - boost::mutex subscriber_mutex_; -}; - -} - -#endif diff --git a/web_video_server/mainpage.dox b/web_video_server/mainpage.dox deleted file mode 100644 index dd3a32b..0000000 --- a/web_video_server/mainpage.dox +++ /dev/null @@ -1,6 +0,0 @@ -/** -\mainpage - -\b A ROS Node to Stream Image Topics Via multiple formats including MJPEG and VP8 -web_video_server converts ROS image streams into an MJPEG or other encoding and streams them via a socket connection. -*/ diff --git a/web_video_server/package.xml b/web_video_server/package.xml deleted file mode 100644 index 6e3a11c..0000000 --- a/web_video_server/package.xml +++ /dev/null @@ -1,35 +0,0 @@ - - - web_video_server - 1.0.0 - HTTP Streaming of ROS Image Topics in Multiple Formats - - Russell Toris - Mitchell Wills - - BSD - - http://ros.org/wiki/web_video_server - https://github.com/RobotWebTools/web_video_server/issues - https://github.com/RobotWebTools/web_video_server - - ament_cmake_ros - - rclcpp - cv_bridge - image_transport - async_web_server_cpp - ffmpeg - sensor_msgs - - rclcpp - cv_bridge - image_transport - async_web_server_cpp - ffmpeg - sensor_msgs - - - ament_cmake - - diff --git a/web_video_server/src/h264_streamer.cpp b/web_video_server/src/h264_streamer.cpp deleted file mode 100644 index a866aa1..0000000 --- a/web_video_server/src/h264_streamer.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "web_video_server/h264_streamer.h" - -namespace web_video_server -{ - -H264Streamer::H264Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "mp4", "libx264", "video/mp4") -{ - /* possible quality presets: - * ultrafast, superfast, veryfast, faster, fast, medium, slow, slower, veryslow, placebo - * no latency improvements observed with ultrafast instead of medium - */ - preset_ = request.get_query_param_value_or_default("preset", "ultrafast"); -} - -H264Streamer::~H264Streamer() -{ -} - -void H264Streamer::initializeEncoder() -{ - av_opt_set(codec_context_->priv_data, "preset", preset_.c_str(), 0); - av_opt_set(codec_context_->priv_data, "tune", "zerolatency", 0); - av_opt_set_int(codec_context_->priv_data, "crf", 20, 0); - av_opt_set_int(codec_context_->priv_data, "bufsize", 100, 0); - av_opt_set_int(codec_context_->priv_data, "keyint", 30, 0); - av_opt_set_int(codec_context_->priv_data, "g", 1, 0); - - // container format options - if (!strcmp(format_context_->oformat->name, "mp4")) { - // set up mp4 for streaming (instead of seekable file output) - av_dict_set(&opt_, "movflags", "+frag_keyframe+empty_moov+faststart", 0); - } -} - -H264StreamerType::H264StreamerType() : - LibavStreamerType("mp4", "libx264", "video/mp4") -{ -} - -boost::shared_ptr H264StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) -{ - return boost::shared_ptr(new H264Streamer(request, connection, nh)); -} - -} diff --git a/web_video_server/src/image_streamer.cpp b/web_video_server/src/image_streamer.cpp deleted file mode 100644 index 05012c2..0000000 --- a/web_video_server/src/image_streamer.cpp +++ /dev/null @@ -1,192 +0,0 @@ -#include "web_video_server/image_streamer.h" -#include -#include - -namespace web_video_server -{ - -ImageStreamer::ImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - request_(request), connection_(connection), nh_(nh), inactive_(false) -{ - topic_ = request.get_query_param_value_or_default("topic", ""); -} - -ImageStreamer::~ImageStreamer() -{ -} - -ImageTransportImageStreamer::ImageTransportImageStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageStreamer(request, connection, nh), it_(nh), initialized_(false) -{ - output_width_ = request.get_query_param_value_or_default("width", -1); - output_height_ = request.get_query_param_value_or_default("height", -1); - invert_ = request.has_query_param("invert"); - default_transport_ = request.get_query_param_value_or_default("default_transport", "raw"); -} - -ImageTransportImageStreamer::~ImageTransportImageStreamer() -{ -} - -void ImageTransportImageStreamer::start() -{ - image_transport::TransportHints hints(nh_.get(), default_transport_); - auto tnat = nh_->get_topic_names_and_types(); - inactive_ = true; - for (auto topic_and_types : tnat) { - if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; - } - auto & topic_name = topic_and_types.first; - if(topic_name == topic_ || (topic_name.find("/") == 0 && topic_name.substr(1) == topic_)){ - inactive_ = false; - break; - } - } - image_sub_ = it_.subscribe(topic_, 1, &ImageTransportImageStreamer::imageCallback, this, &hints); -} - -void ImageTransportImageStreamer::initialize(const cv::Mat &) -{ -} - -void ImageTransportImageStreamer::restreamFrame(double max_age) -{ - if (inactive_ || !initialized_ ) - return; - try { - if ( last_frame + rclcpp::Duration::from_seconds(max_age) < nh_->now() ) { - boost::mutex::scoped_lock lock(send_mutex_); - sendImage(output_size_image, nh_->now() ); // don't update last_frame, it may remain an old value. - } - } - catch (boost::system::system_error &e) - { - // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); - inactive_ = true; - return; - } - catch (std::exception &e) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); - inactive_ = true; - return; - } - catch (...) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); - inactive_ = true; - return; - } -} - -void ImageTransportImageStreamer::imageCallback(const sensor_msgs::msg::Image::ConstSharedPtr &msg) -{ - if (inactive_) - return; - - cv::Mat img; - try - { - if (msg->encoding.find("F") != std::string::npos) - { - // scale floating point images - cv::Mat float_image_bridge = cv_bridge::toCvCopy(msg, msg->encoding)->image; - cv::Mat_ float_image = float_image_bridge; - double max_val; - cv::minMaxIdx(float_image, 0, &max_val); - - if (max_val > 0) - { - float_image *= (255 / max_val); - } - img = float_image; - } - else - { - // Convert to OpenCV native BGR color - img = cv_bridge::toCvCopy(msg, "bgr8")->image; - } - - int input_width = img.cols; - int input_height = img.rows; - - if (output_width_ == -1) - output_width_ = input_width; - if (output_height_ == -1) - output_height_ = input_height; - - if (invert_) - { - // Rotate 180 degrees - cv::flip(img, img, false); - cv::flip(img, img, true); - } - - boost::mutex::scoped_lock lock(send_mutex_); // protects output_size_image - if (output_width_ != input_width || output_height_ != input_height) - { - cv::Mat img_resized; - cv::Size new_size(output_width_, output_height_); - cv::resize(img, img_resized, new_size); - output_size_image = img_resized; - } - else - { - output_size_image = img; - } - - if (!initialized_) - { - initialize(output_size_image); - initialized_ = true; - } - - last_frame = nh_->now(); - sendImage(output_size_image, last_frame ); - - } - catch (cv_bridge::Exception &e) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what()); - inactive_ = true; - return; - } - catch (cv::Exception &e) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "cv_bridge exception: %s", e.what()); - inactive_ = true; - return; - } - catch (boost::system::system_error &e) - { - // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); - inactive_ = true; - return; - } - catch (std::exception &e) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); - inactive_ = true; - return; - } - catch (...) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); - inactive_ = true; - return; - } -} - -} diff --git a/web_video_server/src/jpeg_streamers.cpp b/web_video_server/src/jpeg_streamers.cpp deleted file mode 100644 index 0c5cf88..0000000 --- a/web_video_server/src/jpeg_streamers.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "web_video_server/jpeg_streamers.h" -#include "async_web_server_cpp/http_reply.hpp" - -namespace web_video_server -{ - -MjpegStreamer::MjpegStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) -{ - quality_ = request.get_query_param_value_or_default("quality", 95); - stream_.sendInitialHeader(); -} - -MjpegStreamer::~MjpegStreamer() -{ - this->inactive_ = true; - boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. -} - -void MjpegStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) -{ - std::vector encode_params; - encode_params.push_back(cv::IMWRITE_JPEG_QUALITY); - encode_params.push_back(quality_); - - std::vector encoded_buffer; - cv::imencode(".jpeg", img, encoded_buffer, encode_params); - - stream_.sendPartAndClear(time, "image/jpeg", encoded_buffer); -} - -boost::shared_ptr MjpegStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) -{ - return boost::shared_ptr(new MjpegStreamer(request, connection, nh)); -} - -std::string MjpegStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) -{ - std::stringstream ss; - ss << ""; - return ss.str(); -} - -JpegSnapshotStreamer::JpegSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh) -{ - quality_ = request.get_query_param_value_or_default("quality", 95); -} - -JpegSnapshotStreamer::~JpegSnapshotStreamer() -{ - this->inactive_ = true; - boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. -} - -void JpegSnapshotStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) -{ - std::vector encode_params; - encode_params.push_back(cv::IMWRITE_JPEG_QUALITY); - encode_params.push_back(quality_); - - std::vector encoded_buffer; - cv::imencode(".jpeg", img, encoded_buffer, encode_params); - - char stamp[20]; - sprintf(stamp, "%.06lf", time.seconds()); - async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) - .header("Connection", "close") - .header("Server", "web_video_server") - .header("Cache-Control", - "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, " - "max-age=0") - .header("X-Timestamp", stamp) - .header("Pragma", "no-cache") - .header("Content-type", "image/jpeg") - .header("Access-Control-Allow-Origin", "*") - .header("Content-Length", - boost::lexical_cast(encoded_buffer.size())) - .write(connection_); - connection_->write_and_clear(encoded_buffer); - inactive_ = true; -} - -} diff --git a/web_video_server/src/libav_streamer.cpp b/web_video_server/src/libav_streamer.cpp deleted file mode 100644 index 28d0425..0000000 --- a/web_video_server/src/libav_streamer.cpp +++ /dev/null @@ -1,383 +0,0 @@ -#include "web_video_server/libav_streamer.h" -#include "async_web_server_cpp/http_reply.hpp" - -/*https://stackoverflow.com/questions/46884682/error-in-building-opencv-with-ffmpeg*/ -#define AV_CODEC_FLAG_GLOBAL_HEADER (1 << 22) -#define CODEC_FLAG_GLOBAL_HEADER AV_CODEC_FLAG_GLOBAL_HEADER - -namespace web_video_server -{ - -static int ffmpeg_boost_mutex_lock_manager(void **mutex, enum AVLockOp op) -{ - if (NULL == mutex) - return -1; - - switch (op) - { - case AV_LOCK_CREATE: - { - *mutex = NULL; - boost::mutex *m = new boost::mutex(); - *mutex = static_cast(m); - break; - } - case AV_LOCK_OBTAIN: - { - boost::mutex *m = static_cast(*mutex); - m->lock(); - break; - } - case AV_LOCK_RELEASE: - { - boost::mutex *m = static_cast(*mutex); - m->unlock(); - break; - } - case AV_LOCK_DESTROY: - { - boost::mutex *m = static_cast(*mutex); - m->lock(); - m->unlock(); - delete m; - break; - } - default: - break; - } - return 0; -} - -LibavStreamer::LibavStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh, - const std::string &format_name, const std::string &codec_name, - const std::string &content_type) : - ImageTransportImageStreamer(request, connection, nh), output_format_(0), format_context_(0), codec_(0), codec_context_(0), video_stream_( - 0), frame_(0), sws_context_(0), first_image_timestamp_(0), format_name_( - format_name), codec_name_(codec_name), content_type_(content_type), opt_(0), io_buffer_(0) -{ - - bitrate_ = request.get_query_param_value_or_default("bitrate", 100000); - qmin_ = request.get_query_param_value_or_default("qmin", 10); - qmax_ = request.get_query_param_value_or_default("qmax", 42); - gop_ = request.get_query_param_value_or_default("gop", 250); - - av_lockmgr_register(&ffmpeg_boost_mutex_lock_manager); - av_register_all(); -} - -LibavStreamer::~LibavStreamer() -{ - if (codec_context_) - avcodec_close(codec_context_); - if (frame_) - { -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - av_free(frame_); - frame_ = NULL; -#else - av_frame_free(&frame_); -#endif - } - if (io_buffer_) - delete io_buffer_; - if (format_context_) { - if (format_context_->pb) - av_free(format_context_->pb); - avformat_free_context(format_context_); - } - if (sws_context_) - sws_freeContext(sws_context_); -} - -// output callback for ffmpeg IO context -static int dispatch_output_packet(void* opaque, uint8_t* buffer, int buffer_size) -{ - async_web_server_cpp::HttpConnectionPtr connection = *((async_web_server_cpp::HttpConnectionPtr*) opaque); - std::vector encoded_frame; - encoded_frame.assign(buffer, buffer + buffer_size); - connection->write_and_clear(encoded_frame); - return 0; // TODO: can this fail? -} - -void LibavStreamer::initialize(const cv::Mat &img) -{ - // Load format - format_context_ = avformat_alloc_context(); - if (!format_context_) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); - throw std::runtime_error("Error allocating ffmpeg format context"); - } - output_format_ = av_guess_format(format_name_.c_str(), NULL, NULL); - if (!output_format_) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); - throw std::runtime_error("Error looking up output format"); - } - format_context_->oformat = output_format_; - - // Set up custom IO callback. - size_t io_buffer_size = 3 * 1024; // 3M seen elsewhere and adjudged good - io_buffer_ = new unsigned char[io_buffer_size]; - AVIOContext* io_ctx = avio_alloc_context(io_buffer_, io_buffer_size, AVIO_FLAG_WRITE, &connection_, NULL, dispatch_output_packet, NULL); - if (!io_ctx) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); - throw std::runtime_error("Error setting up IO context"); - } - io_ctx->seekable = 0; // no seeking, it's a stream - format_context_->pb = io_ctx; - output_format_->flags |= AVFMT_FLAG_CUSTOM_IO; - output_format_->flags |= AVFMT_NOFILE; - - // Load codec - if (codec_name_.empty()) // use default codec if none specified - codec_ = avcodec_find_encoder(output_format_->video_codec); - else - codec_ = avcodec_find_encoder_by_name(codec_name_.c_str()); - if (!codec_) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); - throw std::runtime_error("Error looking up codec"); - } - video_stream_ = avformat_new_stream(format_context_, codec_); - if (!video_stream_) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); - throw std::runtime_error("Error creating video stream"); - } - codec_context_ = video_stream_->codec; - - // Set options - avcodec_get_context_defaults3(codec_context_, codec_); - - codec_context_->codec_id = codec_->id; - codec_context_->bit_rate = bitrate_; - - codec_context_->width = output_width_; - codec_context_->height = output_height_; - codec_context_->delay = 0; - - video_stream_->time_base.num = 1; - video_stream_->time_base.den = 1000; - - codec_context_->time_base.num = 1; - codec_context_->time_base.den = 1; - codec_context_->gop_size = gop_; - codec_context_->pix_fmt = AV_PIX_FMT_YUV420P; - codec_context_->max_b_frames = 0; - - // Quality settings - codec_context_->qmin = qmin_; - codec_context_->qmax = qmax_; - - initializeEncoder(); - - // Some formats want stream headers to be separate - if (format_context_->oformat->flags & AVFMT_GLOBALHEADER) - codec_context_->flags |= CODEC_FLAG_GLOBAL_HEADER; - - // Open Codec - if (avcodec_open2(codec_context_, codec_, NULL) < 0) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); - throw std::runtime_error("Could not open video codec"); - } - - // Allocate frame buffers -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - frame_ = avcodec_alloc_frame(); -#else - frame_ = av_frame_alloc(); -#endif - av_image_alloc(frame_->data, frame_->linesize, output_width_, output_height_, - codec_context_->pix_fmt, 1); - - frame_->width = output_width_; - frame_->height = output_height_; - frame_->format = codec_context_->pix_fmt; - output_format_->flags |= AVFMT_NOFILE; - - // Generate header - std::vector header_buffer; - std::size_t header_size; - uint8_t *header_raw_buffer; - // define meta data - av_dict_set(&format_context_->metadata, "author", "ROS web_video_server", 0); - av_dict_set(&format_context_->metadata, "title", topic_.c_str(), 0); - - // Send response headers - async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header( - "Server", "web_video_server").header("Cache-Control", - "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").header( - "Pragma", "no-cache").header("Expires", "0").header("Max-Age", "0").header("Trailer", "Expires").header( - "Content-type", content_type_).header("Access-Control-Allow-Origin", "*").write(connection_); - - // Send video stream header - if (avformat_write_header(format_context_, &opt_) < 0) - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::internal_server_error)(request_, - connection_, - NULL, NULL); - throw std::runtime_error("Error openning dynamic buffer"); - } -} - -void LibavStreamer::initializeEncoder() -{ -} - -void LibavStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) -{ - boost::mutex::scoped_lock lock(encode_mutex_); - if (0 == first_image_timestamp_.nanoseconds()) - { - first_image_timestamp_ = time; - } - std::vector encoded_frame; -#if (LIBAVUTIL_VERSION_MAJOR < 53) - PixelFormat input_coding_format = PIX_FMT_BGR24; -#else - AVPixelFormat input_coding_format = AV_PIX_FMT_BGR24; -#endif - -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - AVPicture *raw_frame = new AVPicture; - avpicture_fill(raw_frame, img.data, input_coding_format, output_width_, output_height_); -#else - AVFrame *raw_frame = av_frame_alloc(); - av_image_fill_arrays(raw_frame->data, raw_frame->linesize, - img.data, input_coding_format, output_width_, output_height_, 1); -#endif - - - - // Convert from opencv to libav - if (!sws_context_) - { - static int sws_flags = SWS_BICUBIC; - sws_context_ = sws_getContext(output_width_, output_height_, input_coding_format, output_width_, output_height_, - codec_context_->pix_fmt, sws_flags, NULL, NULL, NULL); - if (!sws_context_) - { - throw std::runtime_error("Could not initialize the conversion context"); - } - } - - - int ret = sws_scale(sws_context_, - (const uint8_t * const *)raw_frame->data, raw_frame->linesize, 0, - output_height_, frame_->data, frame_->linesize); - -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - delete raw_frame; -#else - av_frame_free(&raw_frame); -#endif - - // Encode the frame - AVPacket pkt; - int got_packet; - av_init_packet(&pkt); - -#if (LIBAVCODEC_VERSION_MAJOR < 54) - int buf_size = 6 * output_width_ * output_height_; - pkt.data = (uint8_t*)av_malloc(buf_size); - pkt.size = avcodec_encode_video(codec_context_, pkt.data, buf_size, frame_); - got_packet = pkt.size > 0; -#elif (LIBAVCODEC_VERSION_MAJOR < 57) - pkt.data = NULL; // packet data will be allocated by the encoder - pkt.size = 0; - if (avcodec_encode_video2(codec_context_, &pkt, frame_, &got_packet) < 0) - { - throw std::runtime_error("Error encoding video frame"); - } -#else - pkt.data = NULL; // packet data will be allocated by the encoder - pkt.size = 0; - if (avcodec_send_frame(codec_context_, frame_) < 0) - { - throw std::runtime_error("Error encoding video frame"); - } - if (avcodec_receive_packet(codec_context_, &pkt) < 0) - { - throw std::runtime_error("Error retrieving encoded packet"); - } -#endif - - if (got_packet) - { - std::size_t size; - uint8_t *output_buf; - - double seconds = (time - first_image_timestamp_).seconds(); - // Encode video at 1/0.95 to minimize delay - pkt.pts = (int64_t)(seconds / av_q2d(video_stream_->time_base) * 0.95); - if (pkt.pts <= 0) - pkt.pts = 1; - pkt.dts = AV_NOPTS_VALUE; - - if (pkt.flags&AV_PKT_FLAG_KEY) - pkt.flags |= AV_PKT_FLAG_KEY; - - pkt.stream_index = video_stream_->index; - - if (av_write_frame(format_context_, &pkt)) - { - throw std::runtime_error("Error when writing frame"); - } - } - else - { - encoded_frame.clear(); - } -#if LIBAVCODEC_VERSION_INT < 54 - av_free(pkt.data); -#endif - -#if LIBAVCODEC_VERSION_INT < AV_VERSION_INT(55,28,1) - av_free_packet(&pkt); -#else - av_packet_unref(&pkt); -#endif - - connection_->write_and_clear(encoded_frame); -} - -LibavStreamerType::LibavStreamerType(const std::string &format_name, const std::string &codec_name, - const std::string &content_type) : - format_name_(format_name), codec_name_(codec_name), content_type_(content_type) -{ -} - -boost::shared_ptr LibavStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) -{ - return boost::shared_ptr( - new LibavStreamer(request, connection, nh, format_name_, codec_name_, content_type_)); -} - -std::string LibavStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) -{ - std::stringstream ss; - ss << ""; - return ss.str(); -} - -} diff --git a/web_video_server/src/multipart_stream.cpp b/web_video_server/src/multipart_stream.cpp deleted file mode 100644 index caf60b7..0000000 --- a/web_video_server/src/multipart_stream.cpp +++ /dev/null @@ -1,84 +0,0 @@ -#include "web_video_server/multipart_stream.h" -#include "async_web_server_cpp/http_reply.hpp" - -namespace web_video_server -{ - -MultipartStream::MultipartStream( - std::function get_now, - async_web_server_cpp::HttpConnectionPtr& connection, - const std::string& boundry, - std::size_t max_queue_size) - : get_now_(get_now), connection_(connection), boundry_(boundry), max_queue_size_(max_queue_size) -{} - -void MultipartStream::sendInitialHeader() { - async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header( - "Server", "web_video_server").header("Cache-Control", - "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").header( - "Pragma", "no-cache").header("Content-type", "multipart/x-mixed-replace;boundary="+boundry_).header( - "Access-Control-Allow-Origin", "*").write(connection_); - connection_->write("--"+boundry_+"\r\n"); -} - -void MultipartStream::sendPartHeader(const rclcpp::Time &time, const std::string& type, size_t payload_size) { - char stamp[20]; - sprintf(stamp, "%.06lf", time.seconds()); - std::shared_ptr > headers( - new std::vector()); - headers->push_back(async_web_server_cpp::HttpHeader("Content-type", type)); - headers->push_back(async_web_server_cpp::HttpHeader("X-Timestamp", stamp)); - headers->push_back( - async_web_server_cpp::HttpHeader("Content-Length", boost::lexical_cast(payload_size))); - connection_->write(async_web_server_cpp::HttpReply::to_buffers(*headers), headers); -} - -void MultipartStream::sendPartFooter(const rclcpp::Time &time) { - std::shared_ptr str(new std::string("\r\n--"+boundry_+"\r\n")); - PendingFooter pf; - pf.timestamp = time; - pf.contents = str; - connection_->write(boost::asio::buffer(*str), str); - if (max_queue_size_ > 0) pending_footers_.push(pf); -} - -void MultipartStream::sendPartAndClear(const rclcpp::Time &time, const std::string& type, - std::vector &data) { - if (!isBusy()) - { - sendPartHeader(time, type, data.size()); - connection_->write_and_clear(data); - sendPartFooter(time); - } -} - -void MultipartStream::sendPart(const rclcpp::Time &time, const std::string& type, - const boost::asio::const_buffer &buffer, - async_web_server_cpp::HttpConnection::ResourcePtr resource) { - if (!isBusy()) - { - sendPartHeader(time, type, boost::asio::buffer_size(buffer)); - connection_->write(buffer, resource); - sendPartFooter(time); - } -} - -bool MultipartStream::isBusy() { - rclcpp::Time currentTime = get_now_(); - while (!pending_footers_.empty()) - { - if (pending_footers_.front().contents.expired()) { - pending_footers_.pop(); - } else { - rclcpp::Time footerTime = pending_footers_.front().timestamp; - if ((currentTime - footerTime).seconds() > 0.5) { - pending_footers_.pop(); - } else { - break; - } - } - } - return !(max_queue_size_ == 0 || pending_footers_.size() < max_queue_size_); -} - -} diff --git a/web_video_server/src/png_streamers.cpp b/web_video_server/src/png_streamers.cpp deleted file mode 100644 index 1a9f874..0000000 --- a/web_video_server/src/png_streamers.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "web_video_server/png_streamers.h" -#include "async_web_server_cpp/http_reply.hpp" - -namespace web_video_server -{ - -PngStreamer::PngStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) -{ - quality_ = request.get_query_param_value_or_default("quality", 3); - stream_.sendInitialHeader(); -} - -PngStreamer::~PngStreamer() -{ - this->inactive_ = true; - boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. -} - -void PngStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) -{ - std::vector encode_params; - encode_params.push_back(cv::IMWRITE_PNG_COMPRESSION); - encode_params.push_back(quality_); - - std::vector encoded_buffer; - cv::imencode(".png", img, encoded_buffer, encode_params); - - stream_.sendPartAndClear(time, "image/png", encoded_buffer); -} - -boost::shared_ptr PngStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) -{ - return boost::shared_ptr(new PngStreamer(request, connection, nh)); -} - -std::string PngStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) -{ - std::stringstream ss; - ss << ""; - return ss.str(); -} - -PngSnapshotStreamer::PngSnapshotStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) : - ImageTransportImageStreamer(request, connection, nh) -{ - quality_ = request.get_query_param_value_or_default("quality", 3); -} - -PngSnapshotStreamer::~PngSnapshotStreamer() -{ - this->inactive_ = true; - boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. -} - -void PngSnapshotStreamer::sendImage(const cv::Mat &img, const rclcpp::Time &time) -{ - std::vector encode_params; - encode_params.push_back(cv::IMWRITE_PNG_COMPRESSION); - encode_params.push_back(quality_); - - std::vector encoded_buffer; - cv::imencode(".png", img, encoded_buffer, encode_params); - - char stamp[20]; - sprintf(stamp, "%.06lf", time.seconds()); - async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok) - .header("Connection", "close") - .header("Server", "web_video_server") - .header("Cache-Control", - "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, " - "max-age=0") - .header("X-Timestamp", stamp) - .header("Pragma", "no-cache") - .header("Content-type", "image/png") - .header("Access-Control-Allow-Origin", "*") - .header("Content-Length", - boost::lexical_cast(encoded_buffer.size())) - .write(connection_); - connection_->write_and_clear(encoded_buffer); - inactive_ = true; -} - -} diff --git a/web_video_server/src/ros_compressed_streamer.cpp b/web_video_server/src/ros_compressed_streamer.cpp deleted file mode 100644 index 0fedaae..0000000 --- a/web_video_server/src/ros_compressed_streamer.cpp +++ /dev/null @@ -1,102 +0,0 @@ -#include "web_video_server/ros_compressed_streamer.h" - -namespace web_video_server -{ - -RosCompressedStreamer::RosCompressedStreamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - ImageStreamer(request, connection, nh), stream_(std::bind(&rclcpp::Node::now, nh), connection) -{ - stream_.sendInitialHeader(); -} - -RosCompressedStreamer::~RosCompressedStreamer() -{ - this->inactive_ = true; - boost::mutex::scoped_lock lock(send_mutex_); // protects sendImage. -} - -void RosCompressedStreamer::start() { - std::string compressed_topic = topic_ + "/compressed"; - image_sub_ = nh_->create_subscription( - compressed_topic, 1, std::bind(&RosCompressedStreamer::imageCallback, this, std::placeholders::_1)); -} - -void RosCompressedStreamer::restreamFrame(double max_age) -{ - if (inactive_ || (last_msg == 0)) - return; - - if ( last_frame + rclcpp::Duration::from_seconds(max_age) < nh_->now() ) { - boost::mutex::scoped_lock lock(send_mutex_); - sendImage(last_msg, nh_->now() ); // don't update last_frame, it may remain an old value. - } -} - -void RosCompressedStreamer::sendImage(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg, - const rclcpp::Time &time) { - try { - std::string content_type; - if(msg->format.find("jpeg") != std::string::npos) { - content_type = "image/jpeg"; - } - else if(msg->format.find("png") != std::string::npos) { - content_type = "image/png"; - } - else { - RCLCPP_WARN(nh_->get_logger(), "Unknown ROS compressed image format: %s", msg->format.c_str()); - return; - } - - stream_.sendPart(time, content_type, boost::asio::buffer(msg->data), msg); - } - catch (boost::system::system_error &e) - { - // happens when client disconnects - RCLCPP_DEBUG(nh_->get_logger(), "system_error exception: %s", e.what()); - inactive_ = true; - return; - } - catch (std::exception &e) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception: %s", e.what()); - inactive_ = true; - return; - } - catch (...) - { - // TODO THROTTLE with 30 - RCLCPP_ERROR(nh_->get_logger(), "exception"); - inactive_ = true; - return; - } -} - - -void RosCompressedStreamer::imageCallback(const sensor_msgs::msg::CompressedImage::ConstSharedPtr msg) { - boost::mutex::scoped_lock lock(send_mutex_); // protects last_msg and last_frame - last_msg = msg; - last_frame = rclcpp::Time(msg->header.stamp); - sendImage(last_msg, last_frame); -} - - -boost::shared_ptr RosCompressedStreamerType::create_streamer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) -{ - return boost::shared_ptr(new RosCompressedStreamer(request, connection, nh)); -} - -std::string RosCompressedStreamerType::create_viewer(const async_web_server_cpp::HttpRequest &request) -{ - std::stringstream ss; - ss << ""; - return ss.str(); -} - - -} diff --git a/web_video_server/src/vp8_streamer.cpp b/web_video_server/src/vp8_streamer.cpp deleted file mode 100644 index f45b7ca..0000000 --- a/web_video_server/src/vp8_streamer.cpp +++ /dev/null @@ -1,91 +0,0 @@ -/********************************************************************* - * - * Software License Agreement (BSD License) - * - * Copyright (c) 2014, Worcester Polytechnic Institute - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Worcester Polytechnic Institute nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - *********************************************************************/ - -#include "web_video_server/vp8_streamer.h" - -namespace web_video_server -{ - -Vp8Streamer::Vp8Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "webm", "libvpx", "video/webm") -{ - quality_ = request.get_query_param_value_or_default("quality", "realtime"); -} -Vp8Streamer::~Vp8Streamer() -{ -} - -void Vp8Streamer::initializeEncoder() -{ - typedef std::map AvOptMap; - AvOptMap av_opt_map; - av_opt_map["quality"] = quality_; - av_opt_map["deadline"] = "1"; - av_opt_map["auto-alt-ref"] = "0"; - av_opt_map["lag-in-frames"] = "1"; - av_opt_map["rc_lookahead"] = "1"; - av_opt_map["drop_frame"] = "1"; - av_opt_map["error-resilient"] = "1"; - - for (AvOptMap::iterator itr = av_opt_map.begin(); itr != av_opt_map.end(); ++itr) - { - av_opt_set(codec_context_->priv_data, itr->first.c_str(), itr->second.c_str(), 0); - } - - // Buffering settings - int bufsize = 10; - codec_context_->rc_buffer_size = bufsize; - codec_context_->rc_initial_buffer_occupancy = bufsize; //bitrate/3; - av_opt_set_int(codec_context_->priv_data, "bufsize", bufsize, 0); - av_opt_set_int(codec_context_->priv_data, "buf-initial", bufsize, 0); - av_opt_set_int(codec_context_->priv_data, "buf-optimal", bufsize, 0); - av_opt_set_int(codec_context_->priv_data, "skip_threshold", 10, 0); -} - -Vp8StreamerType::Vp8StreamerType() : - LibavStreamerType("webm", "libvpx", "video/webm") -{ -} - -boost::shared_ptr Vp8StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) -{ - return boost::shared_ptr(new Vp8Streamer(request, connection, nh)); -} - -} diff --git a/web_video_server/src/vp9_streamer.cpp b/web_video_server/src/vp9_streamer.cpp deleted file mode 100644 index 8fcefd8..0000000 --- a/web_video_server/src/vp9_streamer.cpp +++ /dev/null @@ -1,38 +0,0 @@ -#include "web_video_server/vp9_streamer.h" - -namespace web_video_server -{ - -Vp9Streamer::Vp9Streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, rclcpp::Node::SharedPtr nh) : - LibavStreamer(request, connection, nh, "webm", "libvpx-vp9", "video/webm") -{ -} -Vp9Streamer::~Vp9Streamer() -{ -} - -void Vp9Streamer::initializeEncoder() -{ - - // codec options set up to provide somehow reasonable performance in cost of poor quality - // should be updated as soon as VP9 encoding matures - av_opt_set_int(codec_context_->priv_data, "pass", 1, 0); - av_opt_set_int(codec_context_->priv_data, "speed", 8, 0); - av_opt_set_int(codec_context_->priv_data, "cpu-used", 4, 0); // 8 is max - av_opt_set_int(codec_context_->priv_data, "crf", 20, 0); // 0..63 (higher is lower quality) -} - -Vp9StreamerType::Vp9StreamerType() : - LibavStreamerType("webm", "libvpx-vp9", "video/webm") -{ -} - -boost::shared_ptr Vp9StreamerType::create_streamer(const async_web_server_cpp::HttpRequest& request, - async_web_server_cpp::HttpConnectionPtr connection, - rclcpp::Node::SharedPtr nh) -{ - return boost::shared_ptr(new Vp9Streamer(request, connection, nh)); -} - -} diff --git a/web_video_server/src/web_video_server.cpp b/web_video_server/src/web_video_server.cpp deleted file mode 100644 index c0eebf2..0000000 --- a/web_video_server/src/web_video_server.cpp +++ /dev/null @@ -1,389 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include - -#include "web_video_server/web_video_server.h" -#include "web_video_server/ros_compressed_streamer.h" -#include "web_video_server/jpeg_streamers.h" -#include "web_video_server/png_streamers.h" -#include "web_video_server/vp8_streamer.h" -#include "web_video_server/h264_streamer.h" -#include "web_video_server/vp9_streamer.h" -#include "async_web_server_cpp/http_reply.hpp" - -using namespace std::chrono_literals; -using namespace boost::placeholders; - -namespace web_video_server -{ - -static bool __verbose; - -static std::string __default_stream_type; - -static bool ros_connection_logger(async_web_server_cpp::HttpServerRequestHandler forward, - const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, - const char* end) -{ - if (__verbose) - { - // TODO reenable - // RCLCPP_INFO(nh->get_logger(), "Handling Request: %s", request.uri.c_str()); - } - try - { - forward(request, connection, begin, end); - return true; - } - catch (std::exception &e) - { - // TODO reenable - // RCLCPP_WARN(nh->get_logger(), "Error Handling Request: %s", e.what()); - return false; - } - return false; -} - -WebVideoServer::WebVideoServer(rclcpp::Node::SharedPtr &nh, rclcpp::Node::SharedPtr &private_nh) : - nh_(nh), handler_group_( - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)) -{ - rclcpp::Parameter parameter; - if (private_nh->get_parameter("port", parameter)) { - port_ = parameter.as_int(); - } else { - port_ = 8080; - } - if (private_nh->get_parameter("verbose", parameter)) { - __verbose = parameter.as_bool(); - } else { - __verbose = true; - } - - if (private_nh->get_parameter("address", parameter)) { - address_ = parameter.as_string(); - } else { - address_ = "0.0.0.0"; - } - - int server_threads; - if (private_nh->get_parameter("server_threads", parameter)) { - server_threads = parameter.as_int(); - } else { - server_threads = 1; - } - - if (private_nh->get_parameter("ros_threads", parameter)) { - ros_threads_ = parameter.as_int(); - } else { - ros_threads_ = 2; - } - if (private_nh->get_parameter("publish_rate", parameter)) { - publish_rate_ = parameter.as_double(); - } else { - publish_rate_ = -1.0; - } - - if (private_nh->get_parameter("default_stream_type", parameter)) { - __default_stream_type = parameter.as_string(); - } else { - __default_stream_type = "mjpeg"; - } - - stream_types_["mjpeg"] = boost::shared_ptr(new MjpegStreamerType()); - stream_types_["png"] = boost::shared_ptr(new PngStreamerType()); - stream_types_["ros_compressed"] = boost::shared_ptr(new RosCompressedStreamerType()); - stream_types_["vp8"] = boost::shared_ptr(new Vp8StreamerType()); - stream_types_["h264"] = boost::shared_ptr(new H264StreamerType()); - stream_types_["vp9"] = boost::shared_ptr(new Vp9StreamerType()); - - handler_group_.addHandlerForPath("/", boost::bind(&WebVideoServer::handle_list_streams, this, _1, _2, _3, _4)); - handler_group_.addHandlerForPath("/stream", boost::bind(&WebVideoServer::handle_stream, this, _1, _2, _3, _4)); - handler_group_.addHandlerForPath("/stream_viewer", - boost::bind(&WebVideoServer::handle_stream_viewer, this, _1, _2, _3, _4)); - handler_group_.addHandlerForPath("/snapshot", boost::bind(&WebVideoServer::handle_snapshot, this, _1, _2, _3, _4)); - - try - { - server_.reset( - new async_web_server_cpp::HttpServer(address_, boost::lexical_cast(port_), - boost::bind(ros_connection_logger, handler_group_, _1, _2, _3, _4), - server_threads)); - } - catch(boost::exception& e) - { - RCLCPP_ERROR(nh_->get_logger(), "Exception when creating the web server! %s:%d", address_.c_str(), port_); - throw; - } -} - -WebVideoServer::~WebVideoServer() -{ -} - -void WebVideoServer::setup_cleanup_inactive_streams() -{ - std::function callback = std::bind(&WebVideoServer::cleanup_inactive_streams, this); - cleanup_timer_ = nh_->create_wall_timer(500ms, callback); -} - -void WebVideoServer::spin() -{ - server_->run(); - RCLCPP_INFO(nh_->get_logger(), "Waiting For connections on %s:%d", address_.c_str(), port_); - rclcpp::executors::MultiThreadedExecutor spinner(rclcpp::ExecutorOptions(), ros_threads_); - spinner.add_node(nh_); - if ( publish_rate_ > 0 ) { - nh_->create_wall_timer(1s / publish_rate_, [this](){restreamFrames(1.0 / publish_rate_);}); - } - spinner.spin(); - server_->stop(); -} - -void WebVideoServer::restreamFrames( double max_age ) -{ - boost::mutex::scoped_lock lock(subscriber_mutex_); - - typedef std::vector >::iterator itr_type; - - for (itr_type itr = image_subscribers_.begin(); itr < image_subscribers_.end(); ++itr) - { - (*itr)->restreamFrame( max_age ); - } -} - -void WebVideoServer::cleanup_inactive_streams() -{ - boost::mutex::scoped_lock lock(subscriber_mutex_, boost::try_to_lock); - if (lock) - { - typedef std::vector >::iterator itr_type; - itr_type new_end = std::partition(image_subscribers_.begin(), image_subscribers_.end(), - !boost::bind(&ImageStreamer::isInactive, _1)); - if (__verbose) - { - for (itr_type itr = new_end; itr < image_subscribers_.end(); ++itr) - { - RCLCPP_INFO(nh_->get_logger(), "Removed Stream: %s", (*itr)->getTopic().c_str()); - } - } - image_subscribers_.erase(new_end, image_subscribers_.end()); - } -} - -bool WebVideoServer::handle_stream(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, - const char* end) -{ - std::string type = request.get_query_param_value_or_default("type", __default_stream_type); - if (stream_types_.find(type) != stream_types_.end()) - { - std::string topic = request.get_query_param_value_or_default("topic", ""); - // Fallback for topics without corresponding compressed topics - if (type == std::string("ros_compressed")) - { - std::string compressed_topic_name = topic + "/compressed"; - auto tnat = nh_->get_topic_names_and_types(); - bool did_find_compressed_topic = false; - for (auto topic_and_types : tnat) { - if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; - } - auto & topic_name = topic_and_types.first; - if(topic_name == compressed_topic_name || (topic_name.find("/") == 0 && topic_name.substr(1) == compressed_topic_name)){ - did_find_compressed_topic = true; - break; - } - } - if (!did_find_compressed_topic) - { - RCLCPP_WARN(nh_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); - type = "mjpeg"; - } - } - boost::shared_ptr streamer = stream_types_[type]->create_streamer(request, connection, nh_); - streamer->start(); - boost::mutex::scoped_lock lock(subscriber_mutex_); - image_subscribers_.push_back(streamer); - } - else - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)(request, connection, begin, - end); - } - return true; -} - -bool WebVideoServer::handle_snapshot(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, - const char* end) -{ - boost::shared_ptr streamer(new JpegSnapshotStreamer(request, connection, nh_)); - streamer->start(); - - boost::mutex::scoped_lock lock(subscriber_mutex_); - image_subscribers_.push_back(streamer); - return true; -} - -bool WebVideoServer::handle_stream_viewer(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, - const char* end) -{ - std::string type = request.get_query_param_value_or_default("type", __default_stream_type); - if (stream_types_.find(type) != stream_types_.end()) - { - std::string topic = request.get_query_param_value_or_default("topic", ""); - // Fallback for topics without corresponding compressed topics - if (type == std::string("ros_compressed")) - { - - std::string compressed_topic_name = topic + "/compressed"; - auto tnat = nh_->get_topic_names_and_types(); - bool did_find_compressed_topic = false; - for (auto topic_and_types : tnat) { - if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; - } - auto & topic_name = topic_and_types.first; - if(topic_name == compressed_topic_name || (topic_name.find("/") == 0 && topic_name.substr(1) == compressed_topic_name)){ - did_find_compressed_topic = true; - break; - } - } - if (!did_find_compressed_topic) - { - RCLCPP_WARN(nh_->get_logger(), "Could not find compressed image topic for %s, falling back to mjpeg", topic.c_str()); - type = "mjpeg"; - } - } - - async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header( - "Server", "web_video_server").header("Content-type", "text/html;").write(connection); - - std::stringstream ss; - ss << "" << topic << ""; - ss << "

" << topic << "

"; - ss << stream_types_[type]->create_viewer(request); - ss << ""; - connection->write(ss.str()); - } - else - { - async_web_server_cpp::HttpReply::stock_reply(async_web_server_cpp::HttpReply::not_found)(request, connection, begin, - end); - } - return true; -} - -bool WebVideoServer::handle_list_streams(const async_web_server_cpp::HttpRequest &request, - async_web_server_cpp::HttpConnectionPtr connection, const char* begin, - const char* end) -{ - std::vector image_topics; - std::vector camera_info_topics; - auto tnat = nh_->get_topic_names_and_types(); - for (auto topic_and_types : tnat) { - if (topic_and_types.second.size() > 1) { - // explicitly avoid topics with more than one type - break; - } - auto & topic_name = topic_and_types.first; - auto & topic_type = topic_and_types.second[0]; // explicitly take the first - // TODO debugging - fprintf(stderr, "topic_type: %s\n", topic_type.c_str()); - if (topic_type == "sensor_msgs/msg/Image") - { - image_topics.push_back(topic_name); - } - else if (topic_type == "sensor_msgs/msg/CameraInfo") - { - camera_info_topics.push_back(topic_name); - } - } - - async_web_server_cpp::HttpReply::builder(async_web_server_cpp::HttpReply::ok).header("Connection", "close").header( - "Server", "web_video_server").header("Cache-Control", - "no-cache, no-store, must-revalidate, pre-check=0, post-check=0, max-age=0").header( - "Pragma", "no-cache").header("Content-type", "text/html;").write(connection); - - connection->write("" - "ROS Image Topic List" - "

Available ROS Image Topics:

"); - connection->write(""); - // Add the rest of the image topics that don't have camera_info. - connection->write(""); - return true; -} - -} - -int main(int argc, char **argv) -{ - rclcpp::init(argc, argv); - auto nh = std::make_shared("web_video_server"); - auto private_nh = std::make_shared("_web_video_server"); - - web_video_server::WebVideoServer server(nh, private_nh); - server.setup_cleanup_inactive_streams(); - server.spin(); - - return (0); -}