mirror of
https://gitlab.fdmci.hva.nl/technische-informatica-sm3/ti-projectten/rooziinuubii79.git
synced 2025-08-05 12:54:57 +00:00
Compare commits
13 Commits
OpenCV
...
869f320446
Author | SHA1 | Date | |
---|---|---|---|
|
869f320446 | ||
|
820cb39781 | ||
|
5c4a0f1e9d | ||
|
e77aa4b2dc | ||
|
b2432ab9cd | ||
|
93167e67f6 | ||
|
3bb40d5929 | ||
|
9689d70104 | ||
|
01535607fc | ||
|
f0637f4ba8 | ||
|
14a62c022c | ||
|
cd374dab81 | ||
|
f9cb54a1cf |
3
.gitignore
vendored
3
.gitignore
vendored
@@ -13,7 +13,7 @@ src/Socket/a.out
|
||||
src/C++/Driver/cmake_install.cmake
|
||||
src/C++/Socket/a.out
|
||||
src/C++/Driver/Makefile
|
||||
vgcore*
|
||||
src/C++/Driver/vgcore*
|
||||
src/C++/Driver/cmake_install.cmake
|
||||
src/C++/Driver/Makefile
|
||||
src/C++/Driver/log
|
||||
@@ -31,4 +31,3 @@ CMakeFiles/
|
||||
Makefile
|
||||
CMakeCache.txt
|
||||
cmake_install.cmake
|
||||
src/C++/OpenCV/main
|
||||
|
@@ -1,20 +0,0 @@
|
||||
# OpenCV
|
||||
## Requirements
|
||||
For the camera we want it to detect what is happening on the video feed and identify it so it can identify dangers.
|
||||
|
||||
|
||||
## Issues
|
||||
|
||||
* OpenCL not grabbing gpu
|
||||
* Solution: https://github.com/Smorodov/Multitarget-tracker/issues/93
|
||||
|
||||
## Installation
|
||||
### Dependencies
|
||||
* glew
|
||||
* opencv
|
||||
|
||||
|
||||
|
||||
|
||||
## Sources
|
||||
* https://github.com/UnaNancyOwen/OpenCVDNNSample/tree/master
|
1
docs/scrum/retrospective/retro_sprint_4.md
Normal file
1
docs/scrum/retrospective/retro_sprint_4.md
Normal file
@@ -0,0 +1 @@
|
||||
# retro sprint 4
|
@@ -6,10 +6,7 @@ set(CMAKE_CXX_STANDARD 23)
|
||||
find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
|
||||
find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
|
||||
|
||||
# Find OpenCV package
|
||||
find_package(OpenCV REQUIRED)
|
||||
find_package(OpenEXR REQUIRED)
|
||||
include_directories(${OpenCV_INCLUDE_DIRS})
|
||||
include_directories(/usr/local/include)
|
||||
|
||||
set(SOURCE_FILES
|
||||
src/KobukiDriver/KobukiParser.cpp
|
||||
@@ -23,4 +20,4 @@ set(SOURCE_FILES
|
||||
add_executable(kobuki_control ${SOURCE_FILES})
|
||||
|
||||
# Link the static libraries
|
||||
target_link_libraries(kobuki_control ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY} ${OpenCV_LIBS} pthread OpenEXR::OpenEXR)
|
||||
target_link_libraries(kobuki_control ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY} pthread)
|
@@ -509,6 +509,32 @@ void CKobuki::doRotation(long double th) {
|
||||
usleep(25 * 1000);
|
||||
}
|
||||
|
||||
// combines navigation to a coordinate and rotation by an angle, performs
|
||||
// movement to the selected coordinate in the robot's coordinate system
|
||||
void CKobuki::goToXy(long double xx, long double yy) {
|
||||
long double th;
|
||||
|
||||
yy = yy * -1;
|
||||
|
||||
th = atan2(yy, xx);
|
||||
doRotation(th);
|
||||
|
||||
long double s = sqrt(pow(xx, 2) + pow(yy, 2));
|
||||
|
||||
// resetnem suradnicovu sustavu robota
|
||||
x = 0;
|
||||
y = 0;
|
||||
iterationCount = 0;
|
||||
theta = 0;
|
||||
|
||||
// std::cout << "mam prejst: " << s << "[m]" << std::endl;
|
||||
|
||||
goStraight(s);
|
||||
|
||||
usleep(25 * 1000);
|
||||
return;
|
||||
}
|
||||
|
||||
/// @brief Makes the robot move forward for 3 seconds
|
||||
/// @param speedvalue How fast it will drive forward from 0 - 1024
|
||||
void CKobuki::forward(int speedvalue) {
|
||||
|
@@ -31,6 +31,7 @@
|
||||
#include <chrono>
|
||||
#include <sstream>
|
||||
#include "KobukiParser.h"
|
||||
#include "graph.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
|
71
src/C++/Driver/src/KobukiDriver/graph.h
Normal file
71
src/C++/Driver/src/KobukiDriver/graph.h
Normal file
@@ -0,0 +1,71 @@
|
||||
#ifndef GRAPH1010
|
||||
#define GRAPH1010
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
#define GRAPH_ENABLED true
|
||||
|
||||
class plot {
|
||||
public:
|
||||
FILE *gp;
|
||||
bool enabled,persist;
|
||||
plot(bool _persist=false,bool _enabled=GRAPH_ENABLED) {
|
||||
enabled=_enabled;
|
||||
persist=_persist;
|
||||
if (enabled) {
|
||||
if(persist)
|
||||
gp=popen("gnuplot -persist","w");
|
||||
else
|
||||
gp=popen("gnuplot","w");
|
||||
}
|
||||
}
|
||||
|
||||
void plot_data(vector<float> x,const char* style="points",const char* title="Data") {
|
||||
if(!enabled)
|
||||
return;
|
||||
fprintf(gp,"set title '%s' \n",title);
|
||||
fprintf(gp,"plot '-' w %s \n",style);
|
||||
for(int k=0;k<x.size();k++) {
|
||||
fprintf(gp,"%f\n",x[k]);
|
||||
}
|
||||
fprintf(gp,"e\n");
|
||||
fflush(gp);
|
||||
}
|
||||
|
||||
void plot_data(vector<float> x,vector<float> y,const char* style="points",const char* title="Data") {
|
||||
if(!enabled)
|
||||
return;
|
||||
fprintf(gp,"set title '%s' \n",title);
|
||||
fprintf(gp,"plot '-' w %s \n",style);
|
||||
for(int k=0;k<x.size();k++) {
|
||||
fprintf(gp,"%f %f \n",x[k],y[k]);
|
||||
}
|
||||
fprintf(gp,"e\n");
|
||||
fflush(gp);
|
||||
}
|
||||
|
||||
~plot() {
|
||||
if(enabled)
|
||||
pclose(gp);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
/*
|
||||
int main(int argc,char **argv) {
|
||||
plot p;
|
||||
for(int a=0;a<100;a++) {
|
||||
vector<float> x,y;
|
||||
for(int k=a;k<a+200;k++) {
|
||||
x.push_back(k);
|
||||
y.push_back(k*k);
|
||||
}
|
||||
p.plot_data(x,y);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
*/
|
||||
|
||||
#endif
|
@@ -5,7 +5,6 @@ MqttClient::MqttClient(const std::string& address, const std::string& clientId,
|
||||
//here all the @PARAMS are getting set for the connection
|
||||
: client_(address, clientId), username_(username), password_(password), callback_(*this) {
|
||||
client_.set_callback(callback_);
|
||||
|
||||
options.set_clean_session(true);
|
||||
options.set_mqtt_version(MQTTVERSION_3_1_1); // For MQTT 3.1.1
|
||||
if (!username_.empty() && !password_.empty()) {
|
||||
|
@@ -1,19 +1,16 @@
|
||||
#include <iostream>
|
||||
#include <cmath>
|
||||
#include <thread>
|
||||
#include "KobukiDriver/graph.h"
|
||||
#include "MQTT/MqttClient.h"
|
||||
#include "KobukiDriver/CKobuki.h"
|
||||
#include <opencv4/opencv2/opencv.hpp>
|
||||
#include <opencv4/opencv2/core.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
CKobuki robot;
|
||||
std::string readMQTT();
|
||||
void parseMQTT(std::string message);
|
||||
void CapnSend();
|
||||
//ip, clientID, username, password
|
||||
MqttClient client("ws://145.92.224.21/ws/", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
|
||||
MqttClient client("mqtt://145.92.224.21:1884", "KobukiRPI", "rpi", "rpiwachtwoordofzo"); // create a client object
|
||||
std::string message = "stop";
|
||||
std::string serializeKobukiData(const TKobukiData &data);
|
||||
void sendKobukiData(TKobukiData &data);
|
||||
@@ -23,7 +20,6 @@ void setup()
|
||||
unsigned char *null_ptr(0);
|
||||
robot.startCommunication("/dev/ttyUSB0", true, null_ptr);
|
||||
//connect mqtt server and sub to commands
|
||||
|
||||
client.connect();
|
||||
client.subscribe("home/commands");
|
||||
}
|
||||
@@ -31,17 +27,15 @@ void setup()
|
||||
int main()
|
||||
{
|
||||
setup();
|
||||
std::thread image (CapnSend);
|
||||
std::thread safety([&]() { robot.robotSafety(&message); });
|
||||
std::thread sendMqtt([&]() { sendKobukiData(robot.parser.data); });
|
||||
|
||||
while(true){
|
||||
parseMQTT(readMQTT());
|
||||
parseMQTT(readMQTT());
|
||||
}
|
||||
|
||||
sendMqtt.join();
|
||||
safety.join();
|
||||
image.join();
|
||||
return 0;
|
||||
}
|
||||
|
||||
std::string readMQTT()
|
||||
@@ -61,7 +55,7 @@ void parseMQTT(std::string message)
|
||||
{
|
||||
if (message == "up")
|
||||
{
|
||||
robot.forward(350);
|
||||
robot.forward(1024);
|
||||
}
|
||||
else if (message == "left")
|
||||
{
|
||||
@@ -73,7 +67,7 @@ void parseMQTT(std::string message)
|
||||
}
|
||||
else if (message == "down")
|
||||
{
|
||||
robot.forward(-350);
|
||||
robot.forward(-800);
|
||||
}
|
||||
else if (message == "stop")
|
||||
{
|
||||
@@ -276,35 +270,6 @@ std::string serializeKobukiData(const TKobukiData &data) {
|
||||
void sendKobukiData(TKobukiData &data) {
|
||||
while (true) {
|
||||
client.publishMessage("kobuki/data", serializeKobukiData(data));
|
||||
std::cout << "Sent data" << std::endl;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
|
||||
}
|
||||
}
|
||||
|
||||
void CapnSend() {
|
||||
VideoCapture cap(0);
|
||||
if (!cap.isOpened()) {
|
||||
cerr << "Error: Could not open camera" << endl;
|
||||
return;
|
||||
}
|
||||
|
||||
Mat frame;
|
||||
while (true) {
|
||||
cap >> frame; // Capture a new image frame
|
||||
if (frame.empty()) {
|
||||
cerr << "Error: Could not capture image" << endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Convert the image to a byte array
|
||||
vector<uchar> buf;
|
||||
imencode(".jpg", frame, buf);
|
||||
auto* enc_msg = reinterpret_cast<unsigned char*>(buf.data());
|
||||
|
||||
// Publish the image data
|
||||
client.publishMessage("kobuki/cam", string(enc_msg, enc_msg + buf.size()));
|
||||
cout << "Sent image" << endl;
|
||||
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(300)); // Send image every 1000ms
|
||||
}
|
||||
}
|
15
src/C++/MQTT/CMakeLists.txt
Normal file
15
src/C++/MQTT/CMakeLists.txt
Normal file
@@ -0,0 +1,15 @@
|
||||
cmake_minimum_required(VERSION 3.10)
|
||||
set(CMAKE_CXX_STANDARD 23)
|
||||
|
||||
# Find the Paho MQTT C++ library
|
||||
find_library(PAHO_MQTTPP_LIBRARY paho-mqttpp3 PATHS /usr/local/lib)
|
||||
find_library(PAHO_MQTT_LIBRARY paho-mqtt3a PATHS /usr/local/lib)
|
||||
|
||||
# Include the headers
|
||||
include_directories(/usr/local/include)
|
||||
|
||||
# Add the executable
|
||||
add_executable(my_program main.cpp)
|
||||
|
||||
# Link the libraries
|
||||
target_link_libraries(my_program ${PAHO_MQTTPP_LIBRARY} ${PAHO_MQTT_LIBRARY})
|
64
src/C++/MQTT/main.cpp
Normal file
64
src/C++/MQTT/main.cpp
Normal file
@@ -0,0 +1,64 @@
|
||||
#include <iostream>
|
||||
#include <mqtt/async_client.h>
|
||||
#include <thread> // For std::this_thread::sleep_for
|
||||
#include <chrono> // For std::chrono::seconds
|
||||
|
||||
// Define the address of the MQTT broker, the client ID, and the topic to subscribe to.
|
||||
const std::string ADDRESS("mqtt://localhost:1883"); // Broker address (Raspberry Pi)
|
||||
const std::string CLIENT_ID("raspberry_pi_client");
|
||||
const std::string TOPIC("home/commands");
|
||||
|
||||
// Define a callback class that handles incoming messages and connection events.
|
||||
class callback : public virtual mqtt::callback {
|
||||
// Called when a message arrives on a subscribed topic.
|
||||
void message_arrived(mqtt::const_message_ptr msg) override {
|
||||
std::cout << "Received message: '" << msg->get_topic()<< "' : " << msg->to_string() << std::endl;
|
||||
}
|
||||
|
||||
// Called when the connection to the broker is lost.
|
||||
void connection_lost(const std::string& cause) override {
|
||||
std::cerr << "Connection lost. Reason: " << cause << std::endl;
|
||||
}
|
||||
|
||||
// Called when a message delivery is complete.
|
||||
void delivery_complete(mqtt::delivery_token_ptr token) override {
|
||||
std::cout << "Message delivered!" << std::endl;
|
||||
}
|
||||
};
|
||||
|
||||
int main() {
|
||||
// Create an MQTT async client and set up the callback class.
|
||||
mqtt::async_client client(ADDRESS, CLIENT_ID);
|
||||
callback cb;
|
||||
client.set_callback(cb);
|
||||
|
||||
// Set up the connection options (such as username and password).
|
||||
mqtt::connect_options connOpts;
|
||||
connOpts.set_clean_session(true);
|
||||
connOpts.set_user_name("ishak");
|
||||
connOpts.set_password("kobuki");
|
||||
connOpts.set_mqtt_version(MQTTVERSION_3_1_1);
|
||||
|
||||
try {
|
||||
// Try to connect to the broker and wait until successful.
|
||||
std::cout << "Connecting to broker..." << std::endl;
|
||||
client.connect(connOpts)->wait(); // Connect with the provided options
|
||||
std::cout << "Connected!" << std::endl;
|
||||
|
||||
// Subscribe to the specified topic and wait for confirmation.
|
||||
std::cout << "Subscribing to topic: " << TOPIC << std::endl;
|
||||
client.subscribe(TOPIC, 1)->wait(); // Subscribe with QoS level 1
|
||||
|
||||
// Keep the program running to continue receiving messages from the broker.
|
||||
while (true) {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1)); // Sleep to reduce CPU usage
|
||||
}
|
||||
|
||||
} catch (const mqtt::exception &exc) {
|
||||
// Catch any MQTT exceptions and display the error message.
|
||||
std::cerr << "Error: " << exc.what() << std::endl;
|
||||
return 1;
|
||||
}
|
||||
|
||||
return 0; // Return 0 to indicate successful execution
|
||||
}
|
@@ -1,44 +0,0 @@
|
||||
cmake_minimum_required( VERSION 3.6 )
|
||||
|
||||
# Require C++11 (or later)
|
||||
set( CMAKE_CXX_STANDARD 23 )
|
||||
set( CMAKE_CXX_STANDARD_REQUIRED ON )
|
||||
set( CMAKE_CXX_EXTENSIONS OFF )
|
||||
set(BUILD_MODE Debug)
|
||||
# Create Project
|
||||
project( Sample )
|
||||
add_executable( YOLOv4 util.h main.cpp )
|
||||
|
||||
# Set StartUp Project
|
||||
set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "YOLOv4" )
|
||||
|
||||
# Find Package
|
||||
# OpenCV
|
||||
find_package( OpenCV REQUIRED )
|
||||
|
||||
if( OpenCV_FOUND )
|
||||
# Additional Include Directories
|
||||
include_directories( ${OpenCV_INCLUDE_DIRS} )
|
||||
|
||||
# Additional Dependencies
|
||||
target_link_libraries( YOLOv4 ${OpenCV_LIBS} )
|
||||
endif()
|
||||
|
||||
# Download Model
|
||||
set( MODEL https://github.com/AlexeyAB/darknet/releases/download/darknet_yolo_v3_optimal/yolov4.weights )
|
||||
file( DOWNLOAD
|
||||
"${MODEL}"
|
||||
"${CMAKE_CURRENT_LIST_DIR}/yolov4.weights"
|
||||
EXPECTED_HASH SHA256=e8a4f6c62188738d86dc6898d82724ec0964d0eb9d2ae0f0a9d53d65d108d562
|
||||
SHOW_PROGRESS
|
||||
)
|
||||
|
||||
|
||||
# Download Config
|
||||
set( CONFIG https://raw.githubusercontent.com/AlexeyAB/darknet/master/cfg/yolov4.cfg )
|
||||
file( DOWNLOAD
|
||||
"${CONFIG}"
|
||||
"${CMAKE_CURRENT_LIST_DIR}/yolov4.cfg"
|
||||
EXPECTED_HASH SHA256=a6d0f8e5c62cc8378384f75a8159b95fa2964d4162e33351b00ac82e0fc46a34
|
||||
SHOW_PROGRESS
|
||||
)
|
Binary file not shown.
@@ -1,80 +0,0 @@
|
||||
person
|
||||
bicycle
|
||||
car
|
||||
motorbike
|
||||
aeroplane
|
||||
bus
|
||||
train
|
||||
truck
|
||||
boat
|
||||
traffic light
|
||||
fire hydrant
|
||||
stop sign
|
||||
parking meter
|
||||
bench
|
||||
bird
|
||||
cat
|
||||
dog
|
||||
horse
|
||||
sheep
|
||||
cow
|
||||
elephant
|
||||
bear
|
||||
zebra
|
||||
giraffe
|
||||
backpack
|
||||
umbrella
|
||||
handbag
|
||||
tie
|
||||
suitcase
|
||||
frisbee
|
||||
skis
|
||||
snowboard
|
||||
sports ball
|
||||
kite
|
||||
baseball bat
|
||||
baseball glove
|
||||
skateboard
|
||||
surfboard
|
||||
tennis racket
|
||||
bottle
|
||||
wine glass
|
||||
cup
|
||||
fork
|
||||
knife
|
||||
spoon
|
||||
bowl
|
||||
banana
|
||||
apple
|
||||
sandwich
|
||||
orange
|
||||
broccoli
|
||||
carrot
|
||||
hot dog
|
||||
pizza
|
||||
donut
|
||||
cake
|
||||
chair
|
||||
sofa
|
||||
pottedplant
|
||||
bed
|
||||
diningtable
|
||||
toilet
|
||||
tvmonitor
|
||||
laptop
|
||||
mouse
|
||||
remote
|
||||
keyboard
|
||||
cell phone
|
||||
microwave
|
||||
oven
|
||||
toaster
|
||||
sink
|
||||
refrigerator
|
||||
book
|
||||
clock
|
||||
vase
|
||||
scissors
|
||||
teddy bear
|
||||
hair drier
|
||||
toothbrush
|
@@ -1,209 +0,0 @@
|
||||
#include <iostream>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <opencv2/dnn.hpp>
|
||||
#include <filesystem>
|
||||
#include <fstream>
|
||||
|
||||
#include "util.h"
|
||||
|
||||
// Helper function to check if a file exists
|
||||
bool fileExists(const std::string &path)
|
||||
{
|
||||
return std::filesystem::exists(path);
|
||||
}
|
||||
|
||||
// Function to read class names from a file
|
||||
std::vector<std::string> _readClassNameList(const std::string &path)
|
||||
{
|
||||
std::vector<std::string> classes;
|
||||
|
||||
// Check if file exists
|
||||
if (!fileExists(path))
|
||||
{
|
||||
throw std::runtime_error("Class names file not found: " + path);
|
||||
}
|
||||
|
||||
// Try to open and read file
|
||||
std::ifstream file(path);
|
||||
if (!file.is_open())
|
||||
{
|
||||
throw std::runtime_error("Unable to open class names file: " + path);
|
||||
}
|
||||
|
||||
std::string line;
|
||||
while (std::getline(file, line))
|
||||
{
|
||||
if (!line.empty())
|
||||
{
|
||||
classes.push_back(line);
|
||||
}
|
||||
}
|
||||
|
||||
if (classes.empty())
|
||||
{
|
||||
throw std::runtime_error("No classes found in file: " + path);
|
||||
}
|
||||
|
||||
return classes;
|
||||
}
|
||||
|
||||
int main(int argc, char *argv[])
|
||||
{
|
||||
try
|
||||
{
|
||||
// Open Video Capture
|
||||
cv::VideoCapture capture = cv::VideoCapture(0);
|
||||
if (!capture.isOpened())
|
||||
{
|
||||
std::cerr << "Failed to open camera device" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Read Class Name List and Color Table
|
||||
const std::string list = "coco.names";
|
||||
const std::vector<std::string> classes = _readClassNameList(list);
|
||||
const std::vector<cv::Scalar> colors = getClassColors(classes.size());
|
||||
|
||||
// Debug: Print the size of the colors vector
|
||||
std::cout << "Number of colors: " << colors.size() << std::endl;
|
||||
|
||||
// Read Darknet
|
||||
const std::string model = "yolov4.weights";
|
||||
const std::string config = "yolov4.cfg";
|
||||
cv::dnn::Net net = cv::dnn::readNet(model, config);
|
||||
if (net.empty())
|
||||
{
|
||||
std::cerr << "Failed to load network" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// Set Preferable Backend
|
||||
net.setPreferableBackend(cv::dnn::DNN_BACKEND_OPENCV);
|
||||
|
||||
// Set Preferable Target
|
||||
net.setPreferableTarget(cv::dnn::DNN_TARGET_OPENCL);
|
||||
|
||||
while (true)
|
||||
{
|
||||
// Read Frame
|
||||
cv::Mat frame;
|
||||
capture >> frame;
|
||||
if (frame.empty())
|
||||
{
|
||||
cv::waitKey(0);
|
||||
break;
|
||||
}
|
||||
if (frame.channels() == 4)
|
||||
{
|
||||
cv::cvtColor(frame, frame, cv::COLOR_BGRA2BGR);
|
||||
}
|
||||
|
||||
// Create Blob from Input Image
|
||||
cv::Mat blob = cv::dnn::blobFromImage(frame, 1 / 255.f, cv::Size(416, 416), cv::Scalar(), true, false);
|
||||
|
||||
// Set Input Blob
|
||||
net.setInput(blob);
|
||||
|
||||
// Run Forward Network
|
||||
std::vector<cv::Mat> detections;
|
||||
net.forward(detections, getOutputsNames(net));
|
||||
|
||||
// Draw Region
|
||||
std::vector<int32_t> class_ids;
|
||||
std::vector<float> confidences;
|
||||
std::vector<cv::Rect> rectangles;
|
||||
for (cv::Mat &detection : detections)
|
||||
{
|
||||
if (detection.empty())
|
||||
{
|
||||
std::cerr << "Detection matrix is empty!" << std::endl;
|
||||
continue;
|
||||
}
|
||||
for (int32_t i = 0; i < detection.rows; i++)
|
||||
{
|
||||
cv::Mat region = detection.row(i);
|
||||
|
||||
// Retrieve Max Confidence and Class Index
|
||||
cv::Mat scores = region.colRange(5, detection.cols);
|
||||
cv::Point class_id;
|
||||
double confidence;
|
||||
cv::minMaxLoc(scores, 0, &confidence, 0, &class_id);
|
||||
|
||||
// Check Confidence
|
||||
constexpr float threshold = 0.2;
|
||||
if (threshold > confidence)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
|
||||
// Retrieve Object Position
|
||||
const int32_t x_center = static_cast<int32_t>(region.at<float>(0) * frame.cols);
|
||||
const int32_t y_center = static_cast<int32_t>(region.at<float>(1) * frame.rows);
|
||||
const int32_t width = static_cast<int32_t>(region.at<float>(2) * frame.cols);
|
||||
const int32_t height = static_cast<int32_t>(region.at<float>(3) * frame.rows);
|
||||
const cv::Rect rectangle = cv::Rect(x_center - (width / 2), y_center - (height / 2), width, height);
|
||||
|
||||
// Add Class ID, Confidence, Rectangle
|
||||
class_ids.push_back(class_id.x);
|
||||
confidences.push_back(confidence);
|
||||
rectangles.push_back(rectangle);
|
||||
}
|
||||
}
|
||||
|
||||
// Remove Overlap Rectangles using Non-Maximum Suppression
|
||||
constexpr float confidence_threshold = 0.5; // Confidence
|
||||
constexpr float nms_threshold = 0.5; // IoU (Intersection over Union)
|
||||
std::vector<int32_t> indices;
|
||||
cv::dnn::NMSBoxes(rectangles, confidences, confidence_threshold, nms_threshold, indices);
|
||||
|
||||
// Draw Rectangle
|
||||
for (const int32_t &index : indices)
|
||||
{
|
||||
// Bounds checking
|
||||
if (class_ids[index] >= colors.size())
|
||||
{
|
||||
std::cerr << "Color index out of bounds: " << class_ids[index] << " (max: " << colors.size() - 1 << ")" << std::endl;
|
||||
continue;
|
||||
}
|
||||
|
||||
const cv::Rect rectangle = rectangles[index];
|
||||
const cv::Scalar color = colors[class_ids[index]];
|
||||
|
||||
// Debug: Print the index and color
|
||||
std::cout << "Drawing rectangle with color index: " << class_ids[index] << std::endl;
|
||||
|
||||
constexpr int32_t thickness = 3;
|
||||
cv::rectangle(frame, rectangle, color, thickness);
|
||||
|
||||
std::string label = classes[class_ids[index]] + ": " + std::to_string(static_cast<int>(confidences[index] * 100)) + "%";
|
||||
|
||||
int baseLine;
|
||||
cv::Size labelSize = cv::getTextSize(label, cv::FONT_HERSHEY_SIMPLEX, 0.5, 1, &baseLine);
|
||||
int top = std::max(rectangle.y, labelSize.height);
|
||||
cv::rectangle(frame, cv::Point(rectangle.x, top - labelSize.height),
|
||||
cv::Point(rectangle.x + labelSize.width, top + baseLine), color, cv::FILLED);
|
||||
cv::putText(frame, label, cv::Point(rectangle.x, top), cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(255, 255, 255), 1);
|
||||
}
|
||||
|
||||
// Show Image
|
||||
cv::imshow("Object Detection", frame);
|
||||
const int32_t key = cv::waitKey(1);
|
||||
if (key == 'q')
|
||||
{
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
cv::destroyAllWindows();
|
||||
return 0;
|
||||
}
|
||||
catch (const std::exception &e)
|
||||
{
|
||||
std::cerr << "Error: " << e.what() << std::endl;
|
||||
return -1;
|
||||
}
|
||||
}
|
||||
|
||||
// cloned and fixed from https://github.com/UnaNancyOwen/OpenCVDNNSample/tree/master
|
@@ -1,61 +0,0 @@
|
||||
#ifndef __UTIL__
|
||||
#define __UTIL__
|
||||
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <fstream>
|
||||
#include <opencv2/dnn.hpp>
|
||||
#include <opencv2/core.hpp>
|
||||
#include <opencv2/highgui.hpp>
|
||||
|
||||
// Get Output Layers Name
|
||||
std::vector<std::string> getOutputsNames( const cv::dnn::Net& net )
|
||||
{
|
||||
static std::vector<std::string> names;
|
||||
if( names.empty() ){
|
||||
std::vector<int32_t> out_layers = net.getUnconnectedOutLayers();
|
||||
std::vector<std::string> layers_names = net.getLayerNames();
|
||||
names.resize( out_layers.size() );
|
||||
for( size_t i = 0; i < out_layers.size(); ++i ){
|
||||
names[i] = layers_names[out_layers[i] - 1];
|
||||
}
|
||||
}
|
||||
return names;
|
||||
}
|
||||
|
||||
// Get Output Layer Type
|
||||
std::string getOutputLayerType( cv::dnn::Net& net )
|
||||
{
|
||||
const std::vector<int32_t> out_layers = net.getUnconnectedOutLayers();
|
||||
const std::string output_layer_type = net.getLayer( out_layers[0] )->type;
|
||||
return output_layer_type;
|
||||
}
|
||||
|
||||
// Read Class Name List
|
||||
std::vector<std::string> readClassNameList( const std::string list_path )
|
||||
{
|
||||
std::vector<std::string> classes;
|
||||
std::ifstream ifs( list_path );
|
||||
if( !ifs.is_open() ){
|
||||
return classes;
|
||||
}
|
||||
std::string class_name = "";
|
||||
while( std::getline( ifs, class_name ) ){
|
||||
classes.push_back( class_name );
|
||||
}
|
||||
return classes;
|
||||
}
|
||||
|
||||
// Get Class Color Table for Visualize
|
||||
std::vector<cv::Scalar> getClassColors( const int32_t number_of_colors )
|
||||
{
|
||||
cv::RNG random;
|
||||
std::vector<cv::Scalar> colors;
|
||||
for( int32_t i = 0; i < number_of_colors; i++ ){
|
||||
cv::Scalar color( random.uniform( 0, 255 ), random.uniform( 0, 255 ), random.uniform( 0, 255 ) );
|
||||
colors.push_back( color );
|
||||
}
|
||||
return colors;
|
||||
}
|
||||
|
||||
#endif // __UTIL__
|
File diff suppressed because it is too large
Load Diff
Binary file not shown.
@@ -1,15 +1,14 @@
|
||||
from flask import Flask, Response, request, render_template, jsonify
|
||||
from flask import Flask, request, render_template, jsonify
|
||||
import paho.mqtt.client as mqtt
|
||||
import mysql.connector
|
||||
|
||||
app = Flask(__name__)
|
||||
|
||||
kobuki_message = "empty"
|
||||
def on_message(client, userdata, message):
|
||||
global kobuki_message, latest_image
|
||||
if message.topic == "kobuki/data":
|
||||
kobuki_message = str(message.payload.decode("utf-8"))
|
||||
elif message.topic == "kobuki/cam":
|
||||
latest_image = message.payload
|
||||
global kobuki_message #set scope for this variable
|
||||
kobuki_message = str(message.payload.decode("utf-8"))
|
||||
print(kobuki_message)
|
||||
|
||||
# Create an MQTT client instance
|
||||
mqtt_client = mqtt.Client()
|
||||
@@ -17,10 +16,18 @@ mqtt_client.username_pw_set("server", "serverwachtwoordofzo")
|
||||
mqtt_client.connect("localhost", 80, 60)
|
||||
mqtt_client.loop_start()
|
||||
mqtt_client.subscribe("kobuki/data")
|
||||
mqtt_client.subscribe("kobuki/cam")
|
||||
|
||||
mqtt_client.on_message = on_message # this lines needs to be under the function definition otherwise it cant find which function it needs to use
|
||||
|
||||
|
||||
cnx = mysql.connector.connect(
|
||||
host="127.0.0.1",
|
||||
port=3306,
|
||||
user="admin",
|
||||
password="kobuki",
|
||||
database="kobuki")
|
||||
|
||||
cnx.close()
|
||||
|
||||
@app.route('/')
|
||||
def index():
|
||||
return render_template('index.html')
|
||||
@@ -49,23 +56,26 @@ def move():
|
||||
def data():
|
||||
return kobuki_message
|
||||
|
||||
|
||||
@app.route('/image')
|
||||
def image():
|
||||
global latest_image
|
||||
if latest_image is not None:
|
||||
return Response(latest_image, mimetype='image/jpeg')
|
||||
else:
|
||||
return "No image available", 404
|
||||
|
||||
|
||||
@app.route('/phpmyadmin/<path:path>')
|
||||
def phpmyadmin_passthrough(path):
|
||||
# Laat Apache deze route direct afhandelen
|
||||
return "", 404
|
||||
|
||||
|
||||
|
||||
@app.route("/database")
|
||||
def database():
|
||||
try:
|
||||
cur = cnx.cursor()
|
||||
cur.execute("SELECT * FROM kobuki_data")
|
||||
rows = cur.fetchall() # Haal alle rijen op uit het resultaat
|
||||
cur.close()
|
||||
|
||||
return str(rows)
|
||||
except Exception as e:
|
||||
print(f"Database error: {e}")
|
||||
return "Er is een fout opgetreden bij het ophalen van de databasegegevens.", 500
|
||||
|
||||
def add_command(command):
|
||||
command_query = "INSERT INTO kobuki (command) VALUES ();"
|
||||
|
||||
if __name__ == '__main__':
|
||||
app.run(debug=True, port=5000)
|
||||
|
@@ -1,25 +1,24 @@
|
||||
document.addEventListener("DOMContentLoaded", function() {
|
||||
document.querySelectorAll(".btn").forEach(button => {
|
||||
button.addEventListener("click", function(event) {
|
||||
event.preventDefault(); // prevents page refresh
|
||||
// Selecteer alle knoppen en voeg een event listener toe aan elke knop
|
||||
document.querySelectorAll(".btn").forEach(button => {
|
||||
button.addEventListener("click", function(event) {
|
||||
event.preventDefault(); // voorkomt pagina-verversing
|
||||
|
||||
// Get the value of the button
|
||||
const direction = event.target.value;
|
||||
// Haal de waarde van de knop op
|
||||
const direction = event.target.value;
|
||||
|
||||
fetch("/move", {
|
||||
method: "POST",
|
||||
headers: {
|
||||
"Content-Type": "application/json"
|
||||
},
|
||||
body: JSON.stringify({ direction: direction })
|
||||
})
|
||||
.then(response => response.json())
|
||||
.then(data => {script
|
||||
console.log("Success:", data);
|
||||
})
|
||||
.catch(error => {
|
||||
console.error("Error:", error);
|
||||
});
|
||||
fetch("/move", {
|
||||
method: "POST",
|
||||
headers: {
|
||||
"Content-Type": "application/json"
|
||||
},
|
||||
body: JSON.stringify({ direction: direction })
|
||||
})
|
||||
.then(response => response.json())
|
||||
.then(data => {
|
||||
console.log("Success:", data);
|
||||
})
|
||||
.catch(error => {
|
||||
console.error("Error:", error);
|
||||
});
|
||||
});
|
||||
|
||||
@@ -35,7 +34,7 @@ document.addEventListener("DOMContentLoaded", function() {
|
||||
const data = await fetchData();
|
||||
const sensorDataContainer = document.getElementById("sensor-data");
|
||||
sensorDataContainer.innerHTML = ""; // Clear previous data
|
||||
// For each object in JSON array, create a new paragraph element and append it to the sensorDataContainer
|
||||
//for each object in json array create a new paragraph element and append it to the sensorDataContainer
|
||||
for (const [key, value] of Object.entries(data)) {
|
||||
const dataElement = document.createElement("p");
|
||||
dataElement.textContent = `${key}: ${value}`;
|
||||
@@ -43,15 +42,6 @@ document.addEventListener("DOMContentLoaded", function() {
|
||||
}
|
||||
}
|
||||
|
||||
// Update the image
|
||||
function updateImage() {
|
||||
var img = document.getElementById("robot-image");
|
||||
img.src = "/image?" + new Date().getTime(); // Add timestamp to avoid caching
|
||||
}
|
||||
|
||||
// Fetch and display sensor data every 5 seconds
|
||||
setInterval(parseData, 1000);
|
||||
|
||||
// Update the image every 5 seconds
|
||||
setInterval(updateImage, 200);
|
||||
setInterval(parseData, 5000);
|
||||
});
|
@@ -167,4 +167,4 @@ th,td {
|
||||
th {
|
||||
background-color: #f2f2f2;
|
||||
text-align: left;
|
||||
}
|
||||
}
|
||||
|
@@ -1,8 +1,6 @@
|
||||
{% extends 'base.html' %}
|
||||
{% block head %}
|
||||
{% extends 'base.html' %} {% block head %}
|
||||
<link rel="stylesheet" href="../static/style.css" />
|
||||
{% endblock %}
|
||||
{% block content %}
|
||||
{% endblock %} {% block content %}
|
||||
<!DOCTYPE html>
|
||||
<html lang="en">
|
||||
<head>
|
||||
@@ -13,8 +11,8 @@
|
||||
</head>
|
||||
<body>
|
||||
<div class="container">
|
||||
<div class="robot-image">
|
||||
<img src="/image" alt="Kobuki Camera Feed" id="robot-image" />
|
||||
<div class="image-section">
|
||||
<img src="kobuki.jpg" alt="Kobuki Robot" id="robot-image" />
|
||||
</div>
|
||||
<div class="button-section">
|
||||
<form id="form" action="/move" method="post">
|
||||
@@ -44,8 +42,7 @@
|
||||
</table>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<script src="../static/script.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
{% endblock %}
|
||||
{% endblock %}
|
||||
|
@@ -1,7 +0,0 @@
|
||||
allow_anonymous false
|
||||
password_file /etc/mosquitto/passwordfile
|
||||
listener 8080
|
||||
protocol websockets
|
||||
|
||||
listener 1884
|
||||
protocol mqtt
|
@@ -1,22 +0,0 @@
|
||||
server {
|
||||
listen 80;
|
||||
server_name 145.92.224.21;
|
||||
|
||||
# Proxy WebSocket connections for MQTT
|
||||
location /ws/ {
|
||||
proxy_pass http://localhost:9001;
|
||||
proxy_http_version 1.1;
|
||||
proxy_set_header Upgrade $http_upgrade;
|
||||
proxy_set_header Connection "upgrade";
|
||||
proxy_set_header Host $host;
|
||||
}
|
||||
|
||||
# Proxy HTTP connections for Flask
|
||||
location / {
|
||||
proxy_pass http://localhost:5000;
|
||||
proxy_set_header Host $host;
|
||||
proxy_set_header X-Real-IP $remote_addr;
|
||||
proxy_set_header X-Forwarded-For $proxy_add_x_forwarded_for;
|
||||
proxy_set_header X-Forwarded-Proto $scheme;
|
||||
}
|
||||
}
|
@@ -1,7 +0,0 @@
|
||||
stream {
|
||||
server {
|
||||
listen 9001;
|
||||
proxy_pass localhost:8080;
|
||||
}
|
||||
}
|
||||
|
25
teamdocumentatie/Ishak/etische_aspecten.md
Normal file
25
teamdocumentatie/Ishak/etische_aspecten.md
Normal file
@@ -0,0 +1,25 @@
|
||||
# Etische aspecten van het project
|
||||
|
||||
## Visie op de ethische aspecten van het Kobuki-project
|
||||
|
||||
Etische aspecten zijn heel belangrijk in het project, al ben ik wel van mening dat je niet alles kan voorkomen en ook kan waarborgen.
|
||||
|
||||
## Privacy
|
||||
|
||||
Als je bijvoorbeeld kijkt naar het gedeelte privacy, dan is het heel moeilijk om te kijken wat je gaat doen met die gegevens. Ik ga een camera gebruiken op de robot om zo te kijken
|
||||
waar de robot is en wat hij allemaal ziet. Als de robot in een brandende huis komt en dan een persoon ziet, is het wel belangrijk om die persoon goed te kunnen zien. Je zou dan niet kunnen zeggen dat je die persoon bijvoorbeeld moet vervagen, want je moet wel kunnen zien wat de status is van die persoon.
|
||||
Ook is het dan belangrijk om te kijken wat je met die gegevens gaat doen, ga je ze opslaan voor eventuele later gebruik of verwijder je ze direct. Het is heel lastig te bepalen wanneer je op zo een moment privacy schendt.
|
||||
|
||||
## Betrouwbaarheid
|
||||
|
||||
Ik vind dat je de betrouwbaarheid van de robot wel moet waarborgen,
|
||||
want als ik de robot in een brandend huis stuur en hij valt uit, dan kan dat heel gevaarlijk zijn voor de persoon die in dat huis zit. Daar vind ik dat je meer rekening mee moet houden dan met de privacy van de persoon. Het is de bedoeling dat de robot hulpmedewerkers gaat helpen en niet hun werk moeilijker maakt.
|
||||
|
||||
## Impact op hulpverleners & maatschappij
|
||||
|
||||
Als meerdere hulpmedewerkers de robot gaan gebruiken en het word een soort van standaard, dan is het wel belangrijk dat de robot betrouwbaar is en dat je erop kan vertrouwen. Het gaat immers om mensenlevens en dat is wel het belangrijkste. Het is uiteindelijk de bedoeling dat de robot hulpverleners zal helpen en niet hun werk lastiger moet maken. Als de robot een standaard hulpmiddel wordt moet hij wel gebruiksvriendelijk zijn en goed kunnen helpen. De robot moet ook zo goed mogelijk werken om zo de vertrouwen te behouden van de mensen. Als de robot fouten blijft maken en niet betrouwbaar is zullen minder mensen het gebruiken. Ik vind dan ook dat de gebruik van de robot heel transparant moet zijn. Hoe word de robot aangestuurd, hoe vergelijkt hij situaties en hoe hij daarmee omgaat.
|
||||
Als je daar al heel duidelijk in bent bouw je al wat sneller vertrouwen van de mensen op.
|
||||
|
||||
Ik vind dat in dit project de ethische aspecten heel belangrijk zijn en dat je daar ook rekening mee moet houden.
|
||||
Bij betrouwbaarheid en de impact die de robot kan hebben op de maatschappij en de hulpverleners moet je wel goed over nadenken.
|
||||
Je werkt immers met mensenlevens en dat is wel het belangrijkste. Privacy is ook heel belangrijk, maar ik vind dat je daar wel wat soepeler mee om kan gaan.
|
BIN
teamdocumentatie/Ishak/etische_aspecten.pdf
Normal file
BIN
teamdocumentatie/Ishak/etische_aspecten.pdf
Normal file
Binary file not shown.
Reference in New Issue
Block a user