To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 74ae13fb authored by tiagos's avatar tiagos
Browse files

merged GUI and Self Loc

parents 4321bf3b 62ee0588
......@@ -35,8 +35,8 @@ find_package(Qt5Gui REQUIRED)
find_package(Qt5Svg REQUIRED)
# GUI -- Add src and includes
set(MY_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/src)
set(MY_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/include)
#set(MY_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/src)
#set(MY_GUI_LIB_PATH_INC ${PROJECT_SOURCE_DIR}/GUI_Qt/CrazyFlyGUI/include)
# StudentGUI -- Add src and includes
set(STUDENT_GUI_LIB_PATH_SRC ${PROJECT_SOURCE_DIR}/GUI_Qt/studentGUI/src)
......@@ -55,21 +55,21 @@ add_definitions(-std=c++11)
# GUI -- Special Qt sources that need to be wrapped before being compiled
# they have the Qt macro QOBJECT inside, the MOC cpp file needs to be done manually
set(SRC_HDRS_QOBJECT_GUI
${MY_GUI_LIB_PATH_INC}/crazyFlyZoneTab.h
${MY_GUI_LIB_PATH_INC}/myGraphicsScene.h
${MY_GUI_LIB_PATH_INC}/myGraphicsView.h
${MY_GUI_LIB_PATH_INC}/mainguiwindow.h
${MY_GUI_LIB_PATH_INC}/rosNodeThread.h
${MY_GUI_LIB_PATH_INC}/CFLinker.h
)
#set(SRC_HDRS_QOBJECT_GUI
# ${MY_GUI_LIB_PATH_INC}/crazyFlyZoneTab.h
# ${MY_GUI_LIB_PATH_INC}/myGraphicsScene.h
# ${MY_GUI_LIB_PATH_INC}/myGraphicsView.h
# ${MY_GUI_LIB_PATH_INC}/mainguiwindow.h
# ${MY_GUI_LIB_PATH_INC}/rosNodeThread.h
# ${MY_GUI_LIB_PATH_INC}/CFLinker.h
# )
# GUI -- wrap UI file and QOBJECT files
qt5_wrap_ui(UIS_HDRS_GUI ${MY_GUI_LIB_PATH_SRC}/mainguiwindow.ui)
qt5_wrap_cpp(SRC_MOC_HDRS_GUI ${SRC_HDRS_QOBJECT_GUI})
#qt5_wrap_ui(UIS_HDRS_GUI ${MY_GUI_LIB_PATH_SRC}/mainguiwindow.ui)
#qt5_wrap_cpp(SRC_MOC_HDRS_GUI ${SRC_HDRS_QOBJECT_GUI})
# GUI -- wrap resource file qrc->rcc
qt5_add_resources(MY_RESOURCE_FILE_RCC ${MY_RESOURCE_FILE_QRC})
#qt5_add_resources(MY_RESOURCE_FILE_RCC ${MY_RESOURCE_FILE_QRC})
......@@ -135,6 +135,7 @@ add_message_files(
CrazyflieDB.msg
UWBAnchor.msg
UWBAnchorArray.msg
CustomControllerYAML.msg
#----------------------------------------------------------------------
DebugMsg.msg
)
......@@ -152,6 +153,7 @@ add_service_files(
CMQuery.srv
CMUpdate.srv
CMCommand.srv
Anchors.srv
)
## Generate actions in the 'action' folder
......@@ -201,7 +203,7 @@ generate_messages(
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include ${MY_GUI_LIB_PATH_INC} # GUI -- include headers from GUI in package
#INCLUDE_DIRS include ${MY_GUI_LIB_PATH_INC} # GUI -- include headers from GUI in package
INCLUDE_DIRS include ${STUDENT_GUI_LIB_PATH_INC} # StudentGUI -- include headers from GUI in package
LIBRARIES
CATKIN_DEPENDS roscpp rospy std_msgs rosbag roslib
......@@ -216,7 +218,7 @@ catkin_package(
## Your package locations should be listed before other locations
# include_directories(include)
include_directories(
${MY_GUI_LIB_PATH_INC} # GUI -- include directory inside GUI folder
# ${MY_GUI_LIB_PATH_INC} # GUI -- include directory inside GUI folder
${STUDENT_GUI_LIB_PATH_INC} # StudentGUI -- include directory inside GUI folder
${catkin_INCLUDE_DIRS}
include
......@@ -242,6 +244,7 @@ add_executable(TeacherService src/TeacherService.cpp)
add_executable(LocalizationServer src/LocalizationServer.cpp)
add_executable(PPSClient src/PPSClient.cpp)
add_executable(Client src/Client.cpp)
add_executable(UWBAnalysis src/UWBAnalysis.cpp)
add_executable(SafeControllerService src/SafeControllerService.cpp)
add_executable(CustomControllerService src/CustomControllerService.cpp)
add_executable(CentralManagerService src/CentralManagerService.cpp src/CrazyflieIO.cpp)
......@@ -252,23 +255,23 @@ add_executable(FollowN_1Service src/FollowN_1Service.cpp)
# GUI -- Add sources here
set(MY_CPP_SOURCES_GUI # compilation of sources
${MY_GUI_LIB_PATH_SRC}/mainguiwindow.cpp
${MY_GUI_LIB_PATH_SRC}/main.cpp
${MY_GUI_LIB_PATH_SRC}/cornergrabber.cpp
${MY_GUI_LIB_PATH_SRC}/crazyFlyZone.cpp
${MY_GUI_LIB_PATH_SRC}/crazyFlyZoneTab.cpp
${MY_GUI_LIB_PATH_SRC}/myGraphicsRectItem.cpp
${MY_GUI_LIB_PATH_SRC}/myGraphicsScene.cpp
${MY_GUI_LIB_PATH_SRC}/myGraphicsView.cpp
${MY_GUI_LIB_PATH_SRC}/tablePiece.cpp
${MY_GUI_LIB_PATH_SRC}/marker.cpp
${MY_GUI_LIB_PATH_SRC}/rosNodeThread.cpp
${MY_GUI_LIB_PATH_SRC}/crazyFly.cpp
${MY_GUI_LIB_PATH_SRC}/CFLinker.cpp
${MY_GUI_LIB_PATH_SRC}/channelLUT.cpp
${MY_GUI_LIB_PATH_SRC}/centerMarker.cpp
)
#set(MY_CPP_SOURCES_GUI # compilation of sources
# ${MY_GUI_LIB_PATH_SRC}/mainguiwindow.cpp
# ${MY_GUI_LIB_PATH_SRC}/main.cpp
# ${MY_GUI_LIB_PATH_SRC}/cornergrabber.cpp
# ${MY_GUI_LIB_PATH_SRC}/crazyFlyZone.cpp
# ${MY_GUI_LIB_PATH_SRC}/crazyFlyZoneTab.cpp
# ${MY_GUI_LIB_PATH_SRC}/myGraphicsRectItem.cpp
# ${MY_GUI_LIB_PATH_SRC}/myGraphicsScene.cpp
# ${MY_GUI_LIB_PATH_SRC}/myGraphicsView.cpp
# ${MY_GUI_LIB_PATH_SRC}/tablePiece.cpp
# ${MY_GUI_LIB_PATH_SRC}/marker.cpp
# ${MY_GUI_LIB_PATH_SRC}/rosNodeThread.cpp
# ${MY_GUI_LIB_PATH_SRC}/crazyFly.cpp
# ${MY_GUI_LIB_PATH_SRC}/CFLinker.cpp
# ${MY_GUI_LIB_PATH_SRC}/channelLUT.cpp
# ${MY_GUI_LIB_PATH_SRC}/centerMarker.cpp
# )
# StudentGUI -- Add sources here
set(MY_CPP_SOURCES_STUDENT_GUI # compilation of sources
......@@ -278,22 +281,35 @@ set(MY_CPP_SOURCES_STUDENT_GUI # compilation of sources
)
# UWBManagerService -- Add sources here
set(MY_CPP_SOURCES_UWBMS # compilation of sources
${PROJECT_SOURCE_DIR}/src/UWBManagerService.cpp
${PROJECT_SOURCE_DIR}/src/SerialUnix.cpp
${PROJECT_SOURCE_DIR}/src/DataListener.cpp
)
#add_executable(SELF_LOC ${MY_CPP_SOURCES_ANCHOR_LOC})
# GUI -- Add executables here
add_executable(my_GUI ${MY_CPP_SOURCES_GUI} ${UIS_HDRS_GUI} ${SRC_MOC_HDRS_GUI} ${MY_RESOURCE_FILE_RCC})
qt5_use_modules(my_GUI Widgets)
#add_executable(my_GUI ${MY_CPP_SOURCES_GUI} ${UIS_HDRS_GUI} ${SRC_MOC_HDRS_GUI} ${MY_RESOURCE_FILE_RCC})
#qt5_use_modules(my_GUI Widgets)
# StudentGUI -- Add executables here
add_executable(student_GUI ${MY_CPP_SOURCES_STUDENT_GUI} ${UIS_HDRS_STUDENT_GUI} ${SRC_MOC_HDRS_STUDENT_GUI})
qt5_use_modules(student_GUI Widgets)
add_executable(UWBManagerService ${MY_CPP_SOURCES_UWBMS})
add_dependencies(ViconDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(LocalizationServer d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(UWBDataPublisher d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(UWBManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(TeacherService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(Client d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(UWBAnalysis d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CustomControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(CentralManagerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -302,7 +318,7 @@ add_dependencies(FollowCrazyflieService d_fall_pps_generate_messages_cpp ${catki
add_dependencies(FollowN_1Service d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# GUI-- dependencies
add_dependencies(my_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
#add_dependencies(my_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
# StudentGUI-- dependencies
add_dependencies(student_GUI d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
......@@ -332,10 +348,14 @@ target_link_libraries(TeacherService ${catkin_LIBRARIES})
target_link_libraries(UWBDataPublisher ${catkin_LIBRARIES})
target_link_libraries(UWBManagerService ${catkin_LIBRARIES})
target_link_libraries(PPSClient ${catkin_LIBRARIES})
target_link_libraries(Client ${catkin_LIBRARIES})
target_link_libraries(UWBAnalysis ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(CustomControllerService ${catkin_LIBRARIES})
......@@ -349,12 +369,12 @@ target_link_libraries(FollowCrazyflieService ${catkin_LIBRARIES})
target_link_libraries(FollowN_1Service ${catkin_LIBRARIES})
# GUI -- link libraries
target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
target_link_libraries(my_GUI Qt5::Svg)
#target_link_libraries(my_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
#target_link_libraries(my_GUI Qt5::Svg)
# target_link_libraries(my_library Qt5::Widgets) # GUI -- let my_library have access to Qt stuff
# target_link_libraries(my_GUI my_library)
target_link_libraries(my_GUI ${catkin_LIBRARIES})
#target_link_libraries(my_GUI ${catkin_LIBRARIES})
# StudentGUI -- link libraries
target_link_libraries(student_GUI Qt5::Widgets) # GUI -- let my_GUI have acesss to Qt stuff
......
#pragma once
#include "SerialUnix.h"
#include "SerialCommon.h"
#include <list>
......@@ -7,12 +8,12 @@
#include <string>
#include <stdio.h>
#include <cmath>
#include <ctime>
#include <chrono>
struct anchor_data
struct anchor_raw_data
{
unsigned int from, to, distance;
anchor_data(unsigned int f, unsigned int t, unsigned int d) : from(f), to(t), distance(d) {}
anchor_raw_data(unsigned int f, unsigned int t, unsigned int d) : from(f), to(t), distance(d) {}
};
struct anchor_position_data
......@@ -29,20 +30,22 @@ public:
DataListener();
~DataListener();
bool checkAndReadData(std::string value);
bool calulateAnchorPosition();
void setOffset(double x, double y, double z);
bool startAnchorLocalistaion();
private:
double _distances[8];
std::list<anchor_data> anchors;
Serial serial;
std::list<anchor_raw_data> anchors_raw;
std::vector<anchor_position_data> anchor_position;
anchor_position_data offset;
unsigned int elapsedTime;
std::clock_t locTimer;
std::chrono::high_resolution_clock::time_point locTimer;
bool finishedLoc;
double _get_distance_average(unsigned int anchor_from, unsigned int anchor_to, int& flag);
void _CF_Debugger(std::string value);
bool _check_data_validity();
double _get_distance_average(unsigned int anchor_from, unsigned int anchor_to);
void _resetLocalisation();
void _LOC_Debugger(std::string value);
bool _startSerialCommunication();
bool _calculateAnchorPosition();
};
\ No newline at end of file
......@@ -24,7 +24,7 @@ public:
void closeConnection();
bool isConnected(const char* port = "");
void writeSerial(const char* buffer);
bool writeSerial(const char* buffer);
int manageSerial();
......
......@@ -5,4 +5,4 @@ angleMargin: 0.6
battery_threshold_while_flying: 2.7 # in V was 2.9
battery_threshold_while_motors_off: 3.34 # in V
battery_polling_period: 500 # in ms
positioning_polling_period: 10 # 10ms
positioning_polling_period: 15 # 10ms
\ No newline at end of file
#equlibrium offset was 41
feedforwardMotor: [46000, 46000, 46000, 46000]
################### VICON Settings
#equlibrium offset
feedforwardMotor: [41000, 41000, 41000, 41000]
#quadratic motor regression equation (a0, a1, a2)
#motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
#feedback gain matrix
#gainMatrixRoll: [0, -1.714330725, 0, 0, -1.337107465, 0, 5.115369735, 0, 0]
#gainMatrixPitch: [1.714330725, 0, 0, 1.337107465, 0, 0, 0, 5.115369735, 0]
gainMatrixRoll: [0, -1.3, 0, 0, -0.9, 0, 4.715369735, 0, 0]
gainMatrixPitch: [1.3, 0, 0, 0.9, 0, 0, 0, 4.715369735, 0]
gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
......@@ -19,7 +16,7 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
defaultSetpoint: [0.0, 0.0, -0.2, 0.0]
defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
#take off and landing parameters (in meters and seconds)
takeOffDistance : 0.4
......@@ -29,4 +26,35 @@ durationLanding : 3
# Liner Trayectory following parameters
#distanceThreshold : 0.5
distanceThreshold : 0.2
################### UWB Settings
#equlibrium offset
#feedforwardMotor: [46000, 46000, 46000, 46000]
#quadratic motor regression equation (a0, a1, a2)
#motorPoly: [5.484560e-4, 1.032633e-6, 2.130295e-11]
#feedback gain matrix
gainMatrixRoll: [0, -1.3, 0, 0, -0.9, 0, 4.715369735, 0, 0]
gainMatrixPitch: [1.3, 0, 0, 0.9, 0, 0, 0, 4.715369735, 0]
#gainMatrixYaw: [0, 0, 0, 0, 0, 0, 0, 0, 2.843099534]
#gainMatrixThrust: [0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0]
#kalman filter
#filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
#estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw)
#defaultSetpoint: [0.0, 0.0, 0.4, 0.0]
#take off and landing parameters (in meters and seconds)
#takeOffDistance : 0.4
#landingDistance : -0.5
#durationTakeOff : 3
#durationLanding : 3
# Liner Trayectory following parameters
distanceThreshold : 0.3
......@@ -56,7 +56,7 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/CustomControllerYAML.h"
//#include "d_fall_pps/CustomControllerYAML.h"
#include <std_msgs/Int32.h>
......
......@@ -8,7 +8,7 @@
#include "ros/ros.h"
#include <ros/package.h>
bool compare_anchor_data(const anchor_data& a, const anchor_data& b)
bool compare_anchor_data(const anchor_raw_data& a, const anchor_raw_data& b)
{
if ((a.from == b.from && a.to < b.to) || a.from < b.from)
return true;
......@@ -18,42 +18,12 @@ bool compare_anchor_data(const anchor_data& a, const anchor_data& b)
DataListener::DataListener()
{
for (unsigned int j = 0; j < 8; j++) {
_distances[j] = 0;
}
offset.x = 0;
offset.y = 0;
offset.z = 0;
elapsedTime = 0;
remove("AnchorPos.yaml");
remove("data.txt");
}
DataListener::~DataListener()
{
}
bool DataListener::checkAndReadData(std::string value)
{
switch (value.at(0))
{
case '#':
_CF_Debugger(value);
return 1;
break;
case '$':
_LOC_Debugger(value);
return 1;
break;
default:
break;
}
return 0;
}
void DataListener::setOffset(double x, double y, double z)
{
printf("Offset now is (%.2lf, %.2lf, %.2lf)\n", x, y, z);
......@@ -62,40 +32,43 @@ void DataListener::setOffset(double x, double y, double z)
offset.z = z;
}
void DataListener::_CF_Debugger(std::string value)
{
unsigned int distances[8];
sscanf(value.c_str(), "#%u/%u/%u/%u/%u/%u/%u/%u/", &distances[0], &distances[1], &distances[2], &distances[3], &distances[4], &distances[5], &distances[6], &distances[7]);
printf("--- DISTANCE: \t%u \t%u \t%u \t%u \t%u \t%u \t%u \t%u\n", distances[0], distances[1], distances[2], distances[3], \
distances[4], distances[5], distances[6], distances[7]);
/*FILE* file = fopen("data.txt", "a");
elapsedTime += distances[7];
fprintf(file, "%u\n", elapsedTime);
fclose(file);*/
}
void DataListener::_LOC_Debugger(std::string value)
{
unsigned int source, dest, distance;
sscanf(value.c_str(), "$%u/%u/%u", &source, &dest, &distance);
distance -= 500; //500;
distance -= 500;
anchors.push_back(anchor_data(source, dest, distance));
anchors_raw.push_back(anchor_raw_data(source, dest, distance));
printf("--- LOC: \t%u \t%u \t%u\n", source, dest, distance);
//Reset Timer
locTimer = std::clock();
locTimer = std::chrono::high_resolution_clock::now();
}
double DataListener::_get_distance_average(unsigned int anchor_from, unsigned int anchor_to, int& flag)
bool DataListener::_check_data_validity()
{
if (anchors_raw.size() == 0)
return false;
for (unsigned int i = 0; i < 5; i++)
{
for (unsigned int j = i + 1; j < 6; j++)
{
if (_get_distance_average(i, j) == -1)
return false;
}
}
return true;
}
double DataListener::_get_distance_average(unsigned int anchor_from, unsigned int anchor_to)
{
unsigned int sum = 0;
unsigned int count = 0;
std::list<anchor_data>::iterator it;
for (it = anchors.begin(); it != anchors.end(); it++)
std::list<anchor_raw_data>::iterator it;
for (it = anchors_raw.begin(); it != anchors_raw.end(); it++)
{
//Not a measurement from and to
if (it->from != anchor_from || it->to != anchor_to)
......@@ -110,14 +83,13 @@ double DataListener::_get_distance_average(unsigned int anchor_from, unsigned in
//FAILED
if (count == 0 || sum == 0)
flag = -1;
return -1;
//Divide by 100 to get it in dm
return ((double)sum / (double)count) / 100.0;
}
////////////////////////////////////////////////////////////
/*
......@@ -126,20 +98,19 @@ THE POSITION OF THE OTHER ANCHORS, BUT HEY, IT WORKS xD
*/
bool DataListener::calulateAnchorPosition()
bool DataListener::_calculateAnchorPosition()
{
//List is not empty and there is a 5s delay after the last received distance
if (anchors.size() == 0 || (std::clock() - locTimer) / ((double)CLOCKS_PER_SEC) < 2)
if (!_check_data_validity())
return false;
anchors.sort(compare_anchor_data);
int flag = 1;
printf("Finding anchor`s position...\n");
anchors_raw.sort(compare_anchor_data);
double d01, d02, d12;
d01 = _get_distance_average(0, 1, flag);
d02 = _get_distance_average(0, 2, flag);
d12 = _get_distance_average(1, 2, flag);
d01 = _get_distance_average(0, 1);
d02 = _get_distance_average(0, 2);
d12 = _get_distance_average(1, 2);
double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));
......@@ -156,67 +127,95 @@ bool DataListener::calulateAnchorPosition()
anchor_position.push_back(anchor_position_data(1, anchor_positions[3], anchor_positions[4], anchor_positions[5]));
anchor_position.push_back(anchor_position_data(2, anchor_positions[6], anchor_positions[7], anchor_positions[8]));
//Error with one of the preset values
if (flag == -1)
return false;
//For anchor n we only need d0n, d1n, d2n
for (unsigned int n = 3; n < 6; n++)
{
int flag = 1;
double anchor_distances[6] = { _get_distance_average(0, n, flag), _get_distance_average(1, n, flag), _get_distance_average(2, n, flag), \
_get_distance_average(0, n, flag), _get_distance_average(1, n, flag), _get_distance_average(2, n, flag)
double anchor_distances[6] = { _get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n), \
_get_distance_average(0, n), _get_distance_average(1, n), _get_distance_average(2, n)
};
if (flag != -1) {
//Calculate position of anchor n using IndoorNewton
double result[3];
IndoorNewton(anchor_positions, anchor_distances, result, 200);
anchor_position.push_back(anchor_position_data(n, result[0], result[1], result[2]));
}
//Calculate position of anchor n using IndoorNewton
double result[3];
IndoorNewton(anchor_positions, anchor_distances, result, 200);
anchor_position.push_back(anchor_position_data(n, result[0], result[1], result[2]));
}
//Save anchor positions to yaml file
ros::NodeHandle nodeHandle("~");
std::vector<float> anchorArr;
//std::vector<float> anchorArr;
std::vector<double> anchorArr;
for (unsigned int i = 0; i < anchor_position.size(); i++)
for (unsigned int i = 0; i < anchor_position.size(); i++)
{
anchorArr = {anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z};
nodeHandle.setParam("anchor" + std::to_string(i), anchorArr);
}
finishedLoc = true;
return true;
}
void DataListener::_resetLocalisation()
{
serial.closeConnection();
anchors_raw.clear();
anchor_position.clear();
offset.x = 0;
offset.y = 0;
offset.z = 0;
finishedLoc = false;
}
bool DataListener::_startSerialCommunication()
{
if (!serial.startConnection("/dev/ttyACM0")) return false;
if (!serial.writeSerial("loc100\r")) return false;
// FILE* file = fopen("AnchorPos.yaml", "w");
// fprintf(file, "anchorLength: 6 # Update this number after adding/removing anchors!!!\n\n");
// fprintf(file, "# Structure: anchorID [x, y, z] (ID is always from 0 to length-1)\n\n");
// printf("%s\n---- ANCHOR POSITIONS ----\n", K_NORMAL);
// printf("---- Without offset\n");
// for (unsigned int i = 0; i < anchor_position.size(); i++)
// printf("anchor%u: [%.2lf, %.2lf, %.2lf]\n", i, anchor_position[i].x, anchor_position[i].y, anchor_position[i].z);
// printf("\n");
// printf("---- With offset: (%.2lf, %.2lf, %.2lf)\n", offset.x, offset.y, offset.z);
// for (unsigned int i = 0; i < anchor_position.size(); i++)
// {
// fprintf(file, "anchor%u: [%.2lf, %.2lf, %.2lf]\n", i, anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z);
// printf("%sanchor%u: [%.2lf, %.2lf, %.2lf]\n", K_WARNING, i, anchor_position[i].x - offset.x, anchor_position[i].y - offset.y, anchor_position[i].z - offset.z);
// }
locTimer = std::chrono::high_resolution_clock::now();
while (std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::high_resolution_clock::now() - locTimer).count() < 2000) {
if (!serial.manageSerial())
return false;
// printf("%s---- Saved anchor positions to AnchorPos.yaml\n", K_NORMAL);
// printf("%s--------------------------\n\n", K_NORMAL);
std::string value = serial.readSerial();
while (value != "") {
if (value.at(0) == '$')
_LOC_Debugger(value);
// fclose(file);
// anchors.clear();
value = serial.readSerial();
}
}