Merge branch 'LUT_to_YAML' into 'uwbmaster'

Lut to yaml merge

See merge request D-FaLL/PandS-System/D-FaLL-System!6

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -71,6 +71,7 @@ add_service_files(
   CMUpdate.srv
   CMCommand.srv
   Anchors.srv
+  CFIndex.srv
 )
 
 ## Generate actions in the 'action' folder

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/TeacherService.h

-#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

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/mainguiwindow.h

@@ -56,6 +56,7 @@
 
 #include "d_fall_pps/CrazyflieDB.h"
 #include "d_fall_pps/CrazyflieEntry.h"
+#include "d_fall_pps/CFIndex.h"
 #include "d_fall_pps/Anchors.h"
 
 #define CMD_CRAZYFLY_MOTORS_OFF   5
@@ -204,7 +205,11 @@ private:
     CFLinker* cf_linker;
     std::vector<UWBMarker*> anchor_markers;
 
+    std::map<std::string, std::string> channel_LUT;
+    std::vector<crazyFly*> CF_vector;
+
     ros::ServiceClient UWBServiceClient;
+    ros::ServiceClient TeacherServiceClient;
 
     ros::Publisher DBChangedPublisher;
     ros::Publisher emergencyStopPublisher;
@@ -236,6 +241,10 @@ private:
     bool updateUWBSettings(bool enableChecked);
     void updateAnchors(const Anchors* const a);
 
+    void loadCFs(ros::NodeHandle& nh);
+
+    //void createCFObject(const CrazyfliePositionData)
+
 };
 
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/include/rosNodeThread.h

@@ -74,6 +74,7 @@ public:
     void messageCallback(const ptrToMessage& p_msg);
     void studentIDCallback(const std_msgs::Int32MultiArray &ids);
     void positionDataCallback(const CrazyfliePositionData& data);
+    //void positionDataCallback(const CrazyflieData& data);
 
     ros::ServiceClient m_read_db_client;
     ros::ServiceClient m_update_db_client;
@@ -84,6 +85,7 @@ signals:
     void newViconData(const ptrToMessage& p_msg);
     void newStudentIDs(const std_msgs::Int32MultiArray &ids);
     void newPositionData(const CrazyfliePositionData& data);
+    //void newPositionData(const CrazyflieData& data);
 
 public slots:
     void run();

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/channelLUT.cpp

@@ -38,7 +38,7 @@ std::map<std::string, std::string> channel_LUT
     // {"CF1", "A12D2"},
     // {"CF2", "E341E"},
     // {"CF3", "4E21A"},
-    {"cfOne", "0/76/2M/E7E7E7E701"},
+    /*{"cfOne", "0/76/2M/E7E7E7E701"},
     {"cfTwo", "0/69/2M"},
     {"cfThree", "0/72/2M"},
     {"cfFour", "0/99/2M"},
@@ -52,4 +52,7 @@ std::map<std::string, std::string> channel_LUT
     {"PPS_CF08", "0/56/2M/E7E7E7E708"},
     {"PPS_CF09", "0/56/2M/E7E7E7E709"},
     {"PPS_CF10", "0/56/2M/E7E7E7E70A"},
+    {"uwbtest", "0/80/2M/E7E7E7E7E7"},
+    {"UWB_CF01", "0/80/250K/E7E7E7E7E7"},
+    {"UWB_CF02", "0/70/2M/E7E7E7E7E7"},*/
 };

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/mainguiwindow.cpp

@@ -252,6 +252,7 @@ void MainGUIWindow::_init()
     ros::NodeHandle nodeHandle("~");
 
     UWBServiceClient = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
+    TeacherServiceClient = nodeHandle.serviceClient<CFIndex>("/TeacherService/Hangar", false);
 
     DBChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("DBChanged", 1);
     emergencyStopPublisher = nodeHandle.advertise<std_msgs::Int32>("emergencyStop", 1);
@@ -285,6 +286,8 @@ void MainGUIWindow::_init()
     invertCheckBoxes.push_back(ui->invA5);
     invertCheckBoxes.push_back(ui->invA6);
 
+    loadCFs(nodeHandle);
+
     #endif
 }
 
@@ -311,7 +314,59 @@ void MainGUIWindow::updateComboBoxes()
 }
 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();
     for(int i = 0; i < crazyflies_vector.size(); i++)
     {
@@ -321,7 +376,7 @@ void MainGUIWindow::updateComboBoxesCFs()
             ui->comboBoxCFs->addItem(qstr);
         }
     }
-    #endif
+    #endif*/
 }
 
 void MainGUIWindow::updateComboBoxesCFZones()
@@ -348,6 +403,8 @@ void MainGUIWindow::updateCFPositions(const CrazyfliePositionData& data)
     bool inScene = false;
     int index;
 
+    //CrazyflieData cfdata = data;
+
     // Reconvert CFPD to CFD
     CrazyflieData cfdata;
     cfdata.crazyflieName = std::to_string(data.id);
@@ -1063,7 +1120,10 @@ void MainGUIWindow::updateAnchors(const Anchors* const a)
     ui->table_anchorPos->setRowCount(0);
 
     for(int i = 0; i < anchor_markers.size(); ++i)
+    {
+        scene->removeItem(anchor_markers[i]);
         delete anchor_markers[i];
+    }
 
     anchor_markers.clear();
 
@@ -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)
 {
+    ROS_ERROR("toggled");
+    //bool checked = checkState == Qt::Checked;
+
     // Send Command to enable UWB to UWBManagerService
     std_msgs::Int32 server_msg;
 
@@ -1259,6 +1367,19 @@ void MainGUIWindow::on_set_invert_button_pressed()
 
 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();
     auto it = channel_LUT.find(key);
     if(it != channel_LUT.end())
@@ -1270,7 +1391,7 @@ void MainGUIWindow::on_comboBoxCFs_currentTextChanged(const QString &arg1)
     else
     {
         ROS_INFO("name not found in LUT");
-    }
+    }*/
 }
 
 

pps_ws/src/d_fall_pps/GUI_Qt/CrazyFlyGUI/src/rosNodeThread.cpp

@@ -82,7 +82,6 @@ bool rosNodeThread::init()
 
     ros::start();
     ros::Time::init();
-    //ros::NodeHandle nh("~");
 
     nh = new ros::NodeHandle("~");
 

pps_ws/src/d_fall_pps/include/TeacherService.h

@@ -37,9 +37,12 @@
 #ifndef TEACHERSERVICE_H
 #define TEACHERSERVICE_H
 
+#include "ros/ros.h"
+
 #include "std_msgs/Int32.h"
 
 #include "d_fall_pps/CrazyflieData.h"
+#include "d_fall_pps/CFIndex.h"
 
 #include "StudentSubscriber.h"
 
@@ -51,15 +54,25 @@ namespace d_fall_pps
 	// and subscriptions
 	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
 	void subscribeLocalization();
 
+	// Loads the CF names and addresses from the Hangar and
+	// stores them in the LUT
+	bool loadCFsFromHangar(ros::NodeHandle& nh);
 
-	/************************************************************
-	 *	Callback		  										*
-	 ***********************************************************/
-	// Callback once the refresh Student ID button is clicked
-	void refreshStudents_callback(const std_msgs::Int32 &msg);
+	bool serveCFList(CFIndex::Request& request, CFIndex::Response& response);
+
+	//void initialiseCFIndex();
+
+
+	ros::Publisher studentIDs_publisher;
+	ros::Publisher CFData_publisher;
+
+	std::map<std::string, std::string> channel_LUT;
 }
 
 #endif //	TEACHERSERVICE_H included

pps_ws/src/d_fall_pps/launch/UWBMaster.launch

@@ -12,10 +12,12 @@
 
 	<!-- 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">
+		<rosparam command="load" file="$(find d_fall_pps)/param/CFHangar.yaml" />
 		<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
 	</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>	
 
 </launch>

pps_ws/src/d_fall_pps/param/AnchorPos.yaml

@@ -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)
 # values in dm, not m!!
 
-anchor0: [-22.3, 6.6, 6.1]
-anchor1: [-10.8, -18.5, 2.5]
-anchor2: [1.1, -10.5, 6.1]
-anchor3: [24.6, -10.1, 6.2]
-anchor4: [24.8, 7.6, 6.1]
-anchor5: [3.3, 13.8, 2.2]
+anchor0: [-18.8, 24.3, 0]
+anchor1: [2.5, -13.82, 0]
+anchor2: [12.89, 24.3, 0]
+anchor3: [18.04, 0.85, -6.83]
+anchor4: [-0.56, 3.95, 17.91]
+anchor5: [-21.15, 2.5, 0]

pps_ws/src/d_fall_pps/param/CFHangar.yaml

+# 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"]
+]

pps_ws/src/d_fall_pps/param/Crazyflie.db

-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

pps_ws/src/d_fall_pps/src/DataListener.cpp

@@ -150,6 +150,49 @@ bool DataListener::_calculateAnchorPosition()
 	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 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, \
 	                               x, -y, 0, \
 	                               d02, 0, 0, \
@@ -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(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(3, anchor_positions[12], anchor_positions[13], anchor_positions[14]));
 
 	//For anchor n we only need d0n, d1n, d2n
 	for (unsigned int n = 3; n < 6; n++)
@@ -172,9 +216,9 @@ bool DataListener::_calculateAnchorPosition()
 
 		//Calculate position of anchor n using IndoorNewton
 		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
@@ -191,9 +235,9 @@ void DataListener::_resetLocalisation()
 	anchors_raw.clear();
 	anchor_position.clear();
 
-	//offset.x = 0;
-	//offset.y = 0;
-	//offset.z = 0;
+	offset.x = 0;
+	offset.y = 0;
+	offset.z = 0;
 
 	finishedLoc = false;
 }

pps_ws/src/d_fall_pps/src/TeacherService.cpp

@@ -40,7 +40,6 @@
 #include "ros/ros.h"
 
 #include "std_msgs/Int32MultiArray.h"
-
 #include "d_fall_pps/CrazyfliePositionData.h"
 
 #include "TeacherService.h"
@@ -60,53 +59,86 @@ int main(int argc, char* argv[])
 	ros::NodeHandle nodeHandle("~");
 
 	ros::NodeHandle namespaceNodehandle = ros::NodeHandle();
-	ros::Subscriber refreshStudents_subscriber = namespaceNodehandle.subscribe("my_GUI/refreshStudents", 1, refreshStudents_callback);
+	ros::Subscriber refreshStudents_subscriber = namespaceNodehandle.subscribe("my_GUI/refreshStudents", 1, refresh_callback);
 
 	studentIDs_publisher = nodeHandle.advertise<std_msgs::Int32MultiArray>("studentIDs", 1);
 	CFData_publisher = nodeHandle.advertise<CrazyfliePositionData>("positionData", 1);
 
+	ros::ServiceServer CFHangar = nodeHandle.advertiseService("Hangar", serveCFList);
+	
+	if(!loadCFsFromHangar(nodeHandle))
+		return -1;
+
+
+	/*for(int i = 0; i < 1000; ++i)
+		initialiseCFIndex();*/
+
     ros::spin();
     return 0;
 }
 
-
-/************************************************************
- *	Function		  										*
- ***********************************************************/
-void d_fall_pps::subscribeLocalization()
+bool d_fall_pps::loadCFsFromHangar(ros::NodeHandle& nh)
 {
-	ros::NodeHandle nodeHandle("~");
+	channel_LUT.clear();
+	XmlRpc::XmlRpcValue cf_list;
+	bool success = nh.getParam("Hangar", cf_list);
 
-	for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
-		(*it).unsubscribe();
-
-	studentSubscribers.clear();
+	if(!success || cf_list.size() == 0)
+	{
+		ROS_ERROR("[TeacherService] No CrazyFlies found in hangar!");
+		return false;
+	}
 
-	for(std::vector<int>::const_iterator it = studentIDs.begin(); it != studentIDs.end(); ++it)
+	for(unsigned int i = 0; i < cf_list.size(); ++i)
 	{
-		ROS_ERROR("Adding %i", (*it));
+		std::string address = cf_list[i][1];
 
-		std::string address = "/";
-		address.append(std::to_string(*it));
-		address.append("/LocalizationServer/LocalizationData");
+		if(address == "")
+		{
+			ROS_ERROR("[TeacherService] Bad Hangar entry with CF %i", i);
+			channel_LUT.clear();
+			return false;
+		}
 
-		StudentSubscriber sub = StudentSubscriber(*it, address);
-		studentSubscribers.push_back(sub);
+		channel_LUT.insert(std::pair<std::string, std::string>(cf_list[i][0], cf_list[i][1]));
 	}
-	// for debugging
-	StudentSubscriber sub = StudentSubscriber(8, "/8/UWBDataPublisher/UWBData");
-	studentSubscribers.push_back(sub);
 
 
-	for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
-		(*it).subscribe(nodeHandle);
+	std::stringstream strm;
+	strm << "\n[TeacherService] " << channel_LUT.size() << " CrazyFlies in hangar:\n";
+
+	for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
+		strm << "\t" << it->first.c_str() << "\n";
+		
+	ROS_INFO_STREAM(strm.str());
+
+	return true;
 }
 
+/*void d_fall_pps::initialiseCFIndex()
+{*/
+	/*ROS_ERROR("init index");
 
-/************************************************************
- *	Callback		  										*
- ***********************************************************/
-void d_fall_pps::refreshStudents_callback(const std_msgs::Int32 &msg)
+	CrazyflieData data;
+	data.crazyflieName = "HELLO";
+	data.x = 0;
+	data.y = 0;
+	data.z = 0;
+	data.roll = 0;
+	data.pitch = 0;
+	data.yaw = 0;
+	data.acquiringTime = 0;
+	data.occluded = false;*/
+
+	//CFData_publisher.publish(data);
+
+	/*for(auto it = channel_LUT.begin(); it != channel_LUT.end(); ++it)
+	{
+
+	}*/
+//}
+
+void d_fall_pps::refresh_callback(const std_msgs::Int32 &msg)
 {
 	// Clear existing buffer
 	studentIDs.clear();
@@ -118,7 +150,7 @@ void d_fall_pps::refreshStudents_callback(const std_msgs::Int32 &msg)
     // iterate through all nodes to find PPSClient's
     for(int i = 0; i < v_str.size(); i++)
     {
-        std::string s = v_str[i];