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 c0b4e29a authored by Yvan Bosshard's avatar Yvan Bosshard
Browse files

Merge branch 'LUT_to_YAML' into 'uwbmaster'

Lut to yaml merge

See merge request D-FaLL/PandS-System/D-FaLL-System!6
parents 0456fe56 b4ee46a2
...@@ -71,6 +71,7 @@ add_service_files( ...@@ -71,6 +71,7 @@ add_service_files(
CMUpdate.srv CMUpdate.srv
CMCommand.srv CMCommand.srv
Anchors.srv Anchors.srv
CFIndex.srv
) )
## Generate actions in the 'action' folder ## Generate actions in the 'action' folder
......
#ifndef TEACHERSERVICE_H
#define TEACHERSERVICE_H
#include "std_msgs/Int32.h"
#include "d_fall_pps/CrazyflieData.h"
#include "StudentSubscriber.h"
//#include "ros/ros.h"
namespace d_fall_pps
{
// Stores the connected student IDs
std::vector<int> studentIDs;
// and subscriptions
std::vector<StudentSubscriber> studentSubscribers;
// Callback once the refresh Student ID button is clicked
void refreshStudents_callback(const std_msgs::Int32 &msg);
// Updates the subscription to the localization data of the students
void subscribeLocalization();
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher;
}
#endif // TEACHERSERVICE_H included
\ No newline at end of file
...@@ -56,6 +56,7 @@ ...@@ -56,6 +56,7 @@
#include "d_fall_pps/CrazyflieDB.h" #include "d_fall_pps/CrazyflieDB.h"
#include "d_fall_pps/CrazyflieEntry.h" #include "d_fall_pps/CrazyflieEntry.h"
#include "d_fall_pps/CFIndex.h"
#include "d_fall_pps/Anchors.h" #include "d_fall_pps/Anchors.h"
#define CMD_CRAZYFLY_MOTORS_OFF 5 #define CMD_CRAZYFLY_MOTORS_OFF 5
...@@ -204,7 +205,11 @@ private: ...@@ -204,7 +205,11 @@ private:
CFLinker* cf_linker; CFLinker* cf_linker;
std::vector<UWBMarker*> anchor_markers; std::vector<UWBMarker*> anchor_markers;
std::map<std::string, std::string> channel_LUT;
std::vector<crazyFly*> CF_vector;
ros::ServiceClient UWBServiceClient; ros::ServiceClient UWBServiceClient;
ros::ServiceClient TeacherServiceClient;
ros::Publisher DBChangedPublisher; ros::Publisher DBChangedPublisher;
ros::Publisher emergencyStopPublisher; ros::Publisher emergencyStopPublisher;
...@@ -236,6 +241,10 @@ private: ...@@ -236,6 +241,10 @@ private:
bool updateUWBSettings(bool enableChecked); bool updateUWBSettings(bool enableChecked);
void updateAnchors(const Anchors* const a); void updateAnchors(const Anchors* const a);
void loadCFs(ros::NodeHandle& nh);
//void createCFObject(const CrazyfliePositionData)
}; };
......
...@@ -74,6 +74,7 @@ public: ...@@ -74,6 +74,7 @@ public:
void messageCallback(const ptrToMessage& p_msg); void messageCallback(const ptrToMessage& p_msg);
void studentIDCallback(const std_msgs::Int32MultiArray &ids); void studentIDCallback(const std_msgs::Int32MultiArray &ids);
void positionDataCallback(const CrazyfliePositionData& data); void positionDataCallback(const CrazyfliePositionData& data);
//void positionDataCallback(const CrazyflieData& data);
ros::ServiceClient m_read_db_client; ros::ServiceClient m_read_db_client;
ros::ServiceClient m_update_db_client; ros::ServiceClient m_update_db_client;
...@@ -84,6 +85,7 @@ signals: ...@@ -84,6 +85,7 @@ signals:
void newViconData(const ptrToMessage& p_msg); void newViconData(const ptrToMessage& p_msg);
void newStudentIDs(const std_msgs::Int32MultiArray &ids); void newStudentIDs(const std_msgs::Int32MultiArray &ids);
void newPositionData(const CrazyfliePositionData& data); void newPositionData(const CrazyfliePositionData& data);
//void newPositionData(const CrazyflieData& data);
public slots: public slots:
void run(); void run();
......
...@@ -38,7 +38,7 @@ std::map<std::string, std::string> channel_LUT ...@@ -38,7 +38,7 @@ std::map<std::string, std::string> channel_LUT
// {"CF1", "A12D2"}, // {"CF1", "A12D2"},
// {"CF2", "E341E"}, // {"CF2", "E341E"},
// {"CF3", "4E21A"}, // {"CF3", "4E21A"},
{"cfOne", "0/76/2M/E7E7E7E701"}, /*{"cfOne", "0/76/2M/E7E7E7E701"},
{"cfTwo", "0/69/2M"}, {"cfTwo", "0/69/2M"},
{"cfThree", "0/72/2M"}, {"cfThree", "0/72/2M"},
{"cfFour", "0/99/2M"}, {"cfFour", "0/99/2M"},
...@@ -52,4 +52,7 @@ std::map<std::string, std::string> channel_LUT ...@@ -52,4 +52,7 @@ std::map<std::string, std::string> channel_LUT
{"PPS_CF08", "0/56/2M/E7E7E7E708"}, {"PPS_CF08", "0/56/2M/E7E7E7E708"},
{"PPS_CF09", "0/56/2M/E7E7E7E709"}, {"PPS_CF09", "0/56/2M/E7E7E7E709"},
{"PPS_CF10", "0/56/2M/E7E7E7E70A"}, {"PPS_CF10", "0/56/2M/E7E7E7E70A"},
{"uwbtest", "0/80/2M/E7E7E7E7E7"},
{"UWB_CF01", "0/80/250K/E7E7E7E7E7"},
{"UWB_CF02", "0/70/2M/E7E7E7E7E7"},*/
}; };
...@@ -252,6 +252,7 @@ void MainGUIWindow::_init() ...@@ -252,6 +252,7 @@ void MainGUIWindow::_init()
ros::NodeHandle nodeHandle("~"); ros::NodeHandle nodeHandle("~");
UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false); UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
TeacherServiceClient = nodeHandle.serviceClient<CFIndex>("/TeacherService/Hangar", false);
DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1); DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1); emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
...@@ -285,6 +286,8 @@ void MainGUIWindow::_init() ...@@ -285,6 +286,8 @@ void MainGUIWindow::_init()
invertCheckBoxes.push_back(ui->invA5); invertCheckBoxes.push_back(ui->invA5);
invertCheckBoxes.push_back(ui->invA6); invertCheckBoxes.push_back(ui->invA6);
loadCFs(nodeHandle);
#endif #endif
} }
...@@ -311,7 +314,59 @@ void MainGUIWindow::updateComboBoxes() ...@@ -311,7 +314,59 @@ void MainGUIWindow::updateComboBoxes()
} }
void MainGUIWindow::updateComboBoxesCFs() void MainGUIWindow::updateComboBoxesCFs()
{ {
#ifdef CATKIN_MAKE ui->comboBoxCFs->clear();
CFIndex cfi;
if(!TeacherServiceClient.call(cfi))
{
ROS_ERROR("[Teacher GUI] could not load CF-Database!");
return;
}
int size = crazyflies_vector.size();
for(int i = 0; i < cfi.response.length; ++i)
{
if(!cf_linker->isCFLinked(cfi.response.data[i]))
{
QString qstr = QString::fromStdString(cfi.response.data[i]);
ui->comboBoxCFs->addItem(qstr);
}
bool found = false;
for(int j = 0; j < size; ++j)
{
if(crazyflies_vector[j]->getName() == cfi.response.data[i])
{
found = true;
break;
}
}
if(!found)
{
CrazyflieData* cfdata = new CrazyflieData();
cfdata->crazyflieName = cfi.response.data[i];
cfdata->x = i * 10;
cfdata->y = 0;
cfdata->z = 0;
cfdata->roll = 0;
cfdata->pitch = 0;
cfdata->yaw = 0;
//cfdata->acquiringTime = data.acquiringTime;
cfdata->occluded = true;
QString filename(":/images/center_rect.svg");
crazyFly* tmp_p_crazyfly = new crazyFly(cfdata, filename);
cf_positionsVector.push_back(tmp_p_crazyfly);
scene->addItem(cf_positionsVector.back());
cf_positionsVector.back()->setAddedToScene(true);
}
}
/*#ifdef CATKIN_MAKE
ui->comboBoxCFs->clear(); ui->comboBoxCFs->clear();
for(int i = 0; i < crazyflies_vector.size(); i++) for(int i = 0; i < crazyflies_vector.size(); i++)
{ {
...@@ -321,7 +376,7 @@ void MainGUIWindow::updateComboBoxesCFs() ...@@ -321,7 +376,7 @@ void MainGUIWindow::updateComboBoxesCFs()
ui->comboBoxCFs->addItem(qstr); ui->comboBoxCFs->addItem(qstr);
} }
} }
#endif #endif*/
} }
void MainGUIWindow::updateComboBoxesCFZones() void MainGUIWindow::updateComboBoxesCFZones()
...@@ -348,6 +403,8 @@ void MainGUIWindow::updateCFPositions(const CrazyfliePositionData& data) ...@@ -348,6 +403,8 @@ void MainGUIWindow::updateCFPositions(const CrazyfliePositionData& data)
bool inScene = false; bool inScene = false;
int index; int index;
//CrazyflieData cfdata = data;
// Reconvert CFPD to CFD // Reconvert CFPD to CFD
CrazyflieData cfdata; CrazyflieData cfdata;
cfdata.crazyflieName = std::to_string(data.id); cfdata.crazyflieName = std::to_string(data.id);
...@@ -1063,7 +1120,10 @@ void MainGUIWindow::updateAnchors(const Anchors* const a) ...@@ -1063,7 +1120,10 @@ void MainGUIWindow::updateAnchors(const Anchors* const a)
ui->table_anchorPos->setRowCount(0); ui->table_anchorPos->setRowCount(0);
for(int i = 0; i < anchor_markers.size(); ++i) for(int i = 0; i < anchor_markers.size(); ++i)
{
scene->removeItem(anchor_markers[i]);
delete anchor_markers[i]; delete anchor_markers[i];
}
anchor_markers.clear(); anchor_markers.clear();
...@@ -1118,8 +1178,56 @@ void MainGUIWindow::updateAnchors(const Anchors* const a) ...@@ -1118,8 +1178,56 @@ void MainGUIWindow::updateAnchors(const Anchors* const a)
} }
} }
void MainGUIWindow::loadCFs(ros::NodeHandle& nh)
{
bool success = true;
unsigned int counter = 0;
XmlRpc::XmlRpcValue my_list;
while(success)
{
std::string param = "CF" + std::to_string(counter);
success = nh.getParam(param, my_list);
++counter;
if(success)
{
channel_LUT.insert(std::pair<std::string, std::string>(my_list[0], my_list[1]));
}
}
--counter;
for (std::map<std::string, std::string>::iterator it=channel_LUT.begin(); it!=channel_LUT.end(); ++it)
ROS_WARN("%s, %s", it->first.c_str(), it->second.c_str());
// TEST
/*XmlRpc::XmlRpcValue my_list;
if(!nodeHandle.getParam("CF_LUT", my_list))
ROS_ERROR("fail");*/
/*else
ROS_ERROR("sucksess: %s: %s", res[0][0].c_str(), res[0][1].c_str());//printf(res.c_str());
*/
//std::vector<std::string, std::string> res;
ROS_WARN("function called");
}
void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked) void MainGUIWindow::on_checkBox_enable_UWB_toggled(bool checked)
{ {
ROS_ERROR("toggled");
//bool checked = checkState == Qt::Checked;
// Send Command to enable UWB to UWBManagerService // Send Command to enable UWB to UWBManagerService
std_msgs::Int32 server_msg; std_msgs::Int32 server_msg;
...@@ -1259,6 +1367,19 @@ void MainGUIWindow::on_set_invert_button_pressed() ...@@ -1259,6 +1367,19 @@ void MainGUIWindow::on_set_invert_button_pressed()
void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1) void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
{ {
CFIndex cfi;
cfi.request.cfName = arg1.toStdString();
if(!TeacherServiceClient.call(cfi))
{
ROS_ERROR("[Teacher GUI] failed to get CF Address!");
return;
}
ui->radioAddress_text->setText(QString::fromStdString(cfi.response.data[0]));
/*ROS_WARN("Argument %s", arg1.toStdString().c_str());
std::string key = arg1.toStdString(); std::string key = arg1.toStdString();
auto it = channel_LUT.find(key); auto it = channel_LUT.find(key);
if(it != channel_LUT.end()) if(it != channel_LUT.end())
...@@ -1270,7 +1391,7 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1) ...@@ -1270,7 +1391,7 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
else else
{ {
ROS_INFO("name not found in LUT"); ROS_INFO("name not found in LUT");
} }*/
} }
......
...@@ -82,7 +82,6 @@ bool rosNodeThread::init() ...@@ -82,7 +82,6 @@ bool rosNodeThread::init()
ros::start(); ros::start();
ros::Time::init(); ros::Time::init();
//ros::NodeHandle nh("~");
nh = new ros::NodeHandle("~"); nh = new ros::NodeHandle("~");
......
...@@ -37,9 +37,12 @@ ...@@ -37,9 +37,12 @@
#ifndef TEACHERSERVICE_H #ifndef TEACHERSERVICE_H
#define TEACHERSERVICE_H #define TEACHERSERVICE_H
#include "ros/ros.h"
#include "std_msgs/Int32.h" #include "std_msgs/Int32.h"
#include "d_fall_pps/CrazyflieData.h" #include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/CFIndex.h"
#include "StudentSubscriber.h" #include "StudentSubscriber.h"
...@@ -51,15 +54,25 @@ namespace d_fall_pps ...@@ -51,15 +54,25 @@ namespace d_fall_pps
// and subscriptions // and subscriptions
std::vector<StudentSubscriber> studentSubscribers; std::vector<StudentSubscriber> studentSubscribers;
// Callback once the refresh Student ID button is clicked
void refresh_callback(const std_msgs::Int32 &msg);
// Updates the subscription to the localization data of the students // Updates the subscription to the localization data of the students
void subscribeLocalization(); void subscribeLocalization();
// Loads the CF names and addresses from the Hangar and
// stores them in the LUT
bool loadCFsFromHangar(ros::NodeHandle& nh);
/************************************************************ bool serveCFList(CFIndex::Request& request, CFIndex::Response& response);
* Callback *
***********************************************************/ //void initialiseCFIndex();
// Callback once the refresh Student ID button is clicked
void refreshStudents_callback(const std_msgs::Int32 &msg);
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher;
std::map<std::string, std::string> channel_LUT;
} }
#endif // TEACHERSERVICE_H included #endif // TEACHERSERVICE_H included
...@@ -12,10 +12,12 @@ ...@@ -12,10 +12,12 @@
<!-- When we have a GUI, this has to be adapted and included --> <!-- When we have a GUI, this has to be adapted and included -->
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI" required="true"> <node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI" required="true">
<rosparam command="load" file="$(find d_fall_pps)/param/CFHangar.yaml" />
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> --> <!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node> </node>
<node pkg="d_fall_pps" name="TeacherService" output="screen" type="TeacherService"> <node pkg="d_fall_pps" name="TeacherService" output="screen" type="TeacherService" required="true">
<rosparam command="load" file="$(find d_fall_pps)/param/CFHangar.yaml" />
</node> </node>
</launch> </launch>
...@@ -3,9 +3,9 @@ anchorLength: 6 # Update this number after adding/removing anchors!!! ...@@ -3,9 +3,9 @@ anchorLength: 6 # Update this number after adding/removing anchors!!!
# Structure: anchorID [x, y, z] (ID is always from 0 to length-1) # Structure: anchorID [x, y, z] (ID is always from 0 to length-1)
# values in dm, not m!! # values in dm, not m!!
anchor0: [-22.3, 6.6, 6.1] anchor0: [-18.8, 24.3, 0]
anchor1: [-10.8, -18.5, 2.5] anchor1: [2.5, -13.82, 0]
anchor2: [1.1, -10.5, 6.1] anchor2: [12.89, 24.3, 0]
anchor3: [24.6, -10.1, 6.2] anchor3: [18.04, 0.85, -6.83]
anchor4: [24.8, 7.6, 6.1] anchor4: [-0.56, 3.95, 17.91]
anchor5: [3.3, 13.8, 2.2] anchor5: [-21.15, 2.5, 0]
# LUT for matching
# CF names with their radio addresses
Hangar: [
["PPS_CF01", "0/8/2M/E7E7E7E702"],
["PPS_CF02", "0/00/00/00000000"],
["UWB_CF01", "0/80/250K/E7E7E7E7E7"],
["UWB_CF02", "0/70/2M/E7E7E7E7E7"]
]
8,UWB_CF02,0/70/2M/E7E7E7E7E7,0,-1,-0.83,-0.2,0.98,0.83,2 9,UWB_CF01,0/80/250K/E7E7E7E7E7,1,0.03,-1.15,-0.2,1.65,0.87,2
8,UWB_CF02,0/70/2M/E7E7E7E7E7,0,-0.552586,-2.72294,-0.2,2.42231,-0.386166,2
...@@ -150,6 +150,49 @@ bool DataListener::_calculateAnchorPosition() ...@@ -150,6 +150,49 @@ bool DataListener::_calculateAnchorPosition()
double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02); double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01))); double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));
/*double d03, d23;
d03 = _get_distance_average(0,3);
d23 = _get_distance_average(2,3);*/
/*double a01, a21;
a01 = asin(-12.8/d01);
a21 = asin(-12.8/d12);
d01 *= cos(a01);
d12 *= cos(a21);*/
/*
d01 *= cos(a01);
d12 *= cos(a21);*/
/*double a03, a23;
a03 = asin(-6/d03);
a23 = asin(-6/d23);
d03 *= cos(a03);
d23 *= cos(a23);
double x3 = (d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02);
double y3 = d03 * sin(acos((d03 * d03 + d02 * d02 - d23 * d23) / (2 * d02 * d03)));
double x = (d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02);
double y = d01 * sin(acos((d01 * d01 + d02 * d02 - d12 * d12) / (2 * d02 * d01)));
double anchor_positions[18] = {0, 0, 0, \
x, -y, 0, \
d02, 0, 0, \
0, 0, 0, \
x3, -y3, -6, \
d02, 0, 0
};
ROS_WARN("x: %f, y: %f", x, -y);*/
double anchor_positions[18] = {0, 0, 0, \ double anchor_positions[18] = {0, 0, 0, \
x, -y, 0, \ x, -y, 0, \
d02, 0, 0, \ d02, 0, 0, \
...@@ -161,6 +204,7 @@ bool DataListener::_calculateAnchorPosition() ...@@ -161,6 +204,7 @@ bool DataListener::_calculateAnchorPosition()
anchor_position.push_back(anchor_position_data(0, anchor_positions[0], anchor_positions[1], anchor_positions[2])); anchor_position.push_back(anchor_position_data(0, anchor_positions[0], anchor_positions[1], anchor_positions[2]));
anchor_position.push_back(anchor_position_data(1, anchor_positions[3], anchor_positions[4], anchor_positions[5])); 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])); anchor_position.push_back(anchor_position_data(2, anchor_positions[6], anchor_positions[7], anchor_positions[8]));
//anchor_position.push_back(anchor_position_data(3, anchor_positions[12], anchor_positions[13], anchor_positions[14]));
//For anchor n we only need d0n, d1n, d2n //For anchor n we only need d0n, d1n, d2n
for (unsigned int n = 3; n < 6; n++) for (unsigned int n = 3; n < 6; n++)
...@@ -172,9 +216,9 @@ bool DataListener::_calculateAnchorPosition() ...@@ -172,9 +216,9 @@ bool DataListener::_calculateAnchorPosition()
//Calculate position of anchor n using IndoorNewton //Calculate position of anchor n using IndoorNewton
double result[3]; double result[3];
IndoorNewton(anchor_positions, anchor_distances, result, 200); IndoorNewton(anchor_positions, anchor_distances, result, 100);
anchor_position.push_back(anchor_position_data(n, result[0], result[1], result[2])); anchor_position.push_back(anchor_position_data(n, result[0], result[1], 0.0));//result[2])); //result[2]
} }
//Save anchor positions to yaml file //Save anchor positions to yaml file
...@@ -191,9 +235,9 @@ void DataListener::_resetLocalisation() ...@@ -191,9 +235,9 @@ void DataListener::_resetLocalisation()
anchors_raw.clear(); anchors_raw.clear();
anchor_position.clear(); anchor_position.clear();
//offset.x = 0; offset.x = 0;
//offset.y = 0; offset.y = 0;
//offset.z = 0; offset.z = 0;
finishedLoc = false; finishedLoc = false;