Commit b83524f5 authored by flashingx's avatar flashingx
Browse files

Added Copyright to some files

parent aac6d0dd
// Marker for the Crazyflie flying with UWB.
//
// Copyright (C) 2018
// Yvan Bosshard byvan @ee.ethz.ch
// Michael Rogenmoser michaero @ee.ethz.ch
// Tiago Salzmann tiagos @ee.ethz.ch
//
// Adapted from centerMarker of Angel Romero.
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef UWBMARKER_H #ifndef UWBMARKER_H
#define UWBMARKER_H #define UWBMARKER_H
...@@ -8,12 +30,12 @@ ...@@ -8,12 +30,12 @@
#include "d_fall_pps/UWBAnchor.h" #include "d_fall_pps/UWBAnchor.h"
using namespace d_fall_pps;
#define ANCHOR_HEIGHT 200 * FROM_MILIMETERS_TO_UNITS * 1.2 #define ANCHOR_HEIGHT 200 * FROM_MILIMETERS_TO_UNITS * 1.2
#define ANCHOR_WIDTH 200 * FROM_MILIMETERS_TO_UNITS * 1.2 #define ANCHOR_WIDTH 200 * FROM_MILIMETERS_TO_UNITS * 1.2
#define ANCHOR_OPACITY 0.7 #define ANCHOR_OPACITY 0.7
using namespace d_fall_pps;
class UWBMarker : public QGraphicsSvgItem class UWBMarker : public QGraphicsSvgItem
{ {
public: public:
......
...@@ -23,8 +23,10 @@ ...@@ -23,8 +23,10 @@
#include <QGridLayout> #include <QGridLayout>
#include <QGraphicsRectItem> #include <QGraphicsRectItem>
#ifdef CATKIN_MAKE #ifdef CATKIN_MAKE
#include <std_msgs/Int32.h>
#include <std_msgs/Int32MultiArray.h>
#include "rosNodeThread.h" #include "rosNodeThread.h"
#include "marker.h" #include "marker.h"
#include "crazyFly.h" #include "crazyFly.h"
...@@ -35,10 +37,6 @@ ...@@ -35,10 +37,6 @@
#include "d_fall_pps/CrazyflieEntry.h" #include "d_fall_pps/CrazyflieEntry.h"
#include "d_fall_pps/Anchors.h" #include "d_fall_pps/Anchors.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int32MultiArray.h>
#define CMD_CRAZYFLY_MOTORS_OFF 5 #define CMD_CRAZYFLY_MOTORS_OFF 5
using namespace d_fall_pps; using namespace d_fall_pps;
......
#include "UWBMarker.h" // Marker for the Crazyflie flying with UWB.
//
// Copyright (C) 2018
// Yvan Bosshard byvan @ee.ethz.ch
// Michael Rogenmoser michaero @ee.ethz.ch
// Tiago Salzmann tiagos @ee.ethz.ch
//
// Adapted from centerMarker of Angel Romero.
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <stdio.h> #include <stdio.h>
#include "UWBMarker.h"
UWBMarker::UWBMarker(const UWBAnchor* const anchor, QString filename, QGraphicsItem* parent) : QGraphicsSvgItem(filename) UWBMarker::UWBMarker(const UWBAnchor* const anchor, QString filename, QGraphicsItem* parent) : QGraphicsSvgItem(filename)
{ {
updateMarker(anchor); updateMarker(anchor);
...@@ -8,8 +31,7 @@ UWBMarker::UWBMarker(const UWBAnchor* const anchor, QString filename, QGraphicsI ...@@ -8,8 +31,7 @@ UWBMarker::UWBMarker(const UWBAnchor* const anchor, QString filename, QGraphicsI
m_height = ANCHOR_HEIGHT; m_height = ANCHOR_HEIGHT;
} }
UWBMarker::~UWBMarker() UWBMarker::~UWBMarker(){}
{}
void UWBMarker::updateMarker(const UWBAnchor* const anchor) void UWBMarker::updateMarker(const UWBAnchor* const anchor)
{ {
...@@ -23,7 +45,6 @@ void UWBMarker::updateMarker(const UWBAnchor* const anchor) ...@@ -23,7 +45,6 @@ void UWBMarker::updateMarker(const UWBAnchor* const anchor)
QRectF UWBMarker::boundingRect() const QRectF UWBMarker::boundingRect() const
{ {
// return QRectF(-original_width/2, -original_height/2, original_width, original_height);
return QRectF(-m_width/2, -m_height/2, m_width, m_height); return QRectF(-m_width/2, -m_height/2, m_width, m_height);
} }
......
// Teacher's GUI main window header. // Teacher's GUI main window.
// Copyright (C) 2017 Angel Romero //
// Copyright (C) 2018
// Yvan Bosshard byvan @ee.ethz.ch
// Michael Rogenmoser michaero @ee.ethz.ch
// Tiago Salzmann tiagos @ee.ethz.ch
//
// Adapted from the GUI of Angel Romero.
// //
// This program is free software: you can redistribute it and/or modify // This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by // it under the terms of the GNU General Public License as published by
...@@ -14,12 +20,8 @@ ...@@ -14,12 +20,8 @@
// You should have received a copy of the GNU General Public License // You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>. // along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "mainguiwindow.h" #include <regex>
#include "ui_mainguiwindow.h" #include <string>
#include "crazyFlyZoneTab.h"
#include "myGraphicsScene.h"
#include "myGraphicsView.h"
#include "channelLUT.h"
#include <QObject> #include <QObject>
#include <QDoubleSpinBox> #include <QDoubleSpinBox>
...@@ -28,9 +30,19 @@ ...@@ -28,9 +30,19 @@
#include <QMetaType> #include <QMetaType>
#include <QDir> #include <QDir>
#include <QShortcut> #include <QShortcut>
#include <regex>
#include "mainguiwindow.h"
#include "ui_mainguiwindow.h"
#include "crazyFlyZoneTab.h"
#include "myGraphicsScene.h"
#include "myGraphicsView.h"
#include "channelLUT.h"
#ifdef CATKIN_MAKE #ifdef CATKIN_MAKE
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>
#include "d_fall_pps/UnlabeledMarker.h" #include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/CMRead.h" #include "d_fall_pps/CMRead.h"
#include "d_fall_pps/CrazyflieEntry.h" #include "d_fall_pps/CrazyflieEntry.h"
...@@ -38,16 +50,8 @@ ...@@ -38,16 +50,8 @@
#include "d_fall_pps/CMCommand.h" #include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h" #include "CentralManagerService.h"
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>
#endif #endif
//#include <iomanip>
#include <string>
#define N_MAX_CRAZYFLIES 20 // protection number #define N_MAX_CRAZYFLIES 20 // protection number
#define UWB_UPDATE_DISABLE 0 #define UWB_UPDATE_DISABLE 0
......
/* // This ROS node is subscribed to the ViconDataPublisher as well as
* // to the UWBDataPublisher. It is used to relay the data from the
* Localization Server // desired localisation source to the controller and the GUI.
* //
* This node is subscribed to the ViconDataPublisher as well // Copyright (C) 2018
* as to the UWBDataPublisher. It is used to compare the // Yvan Bosshard byvan @ee.ethz.ch
* data and relay the desired localization source to the // Michael Rogenmoser michaero @ee.ethz.ch
* PPSClient. // Tiago Salzmann tiagos @ee.ethz.ch
* //
*/ // This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef LOCALIZATIONSERVER_H #ifndef LOCALIZATIONSERVER_H
#define LOCALIZATIONSERVER_H #define LOCALIZATIONSERVER_H
...@@ -27,9 +38,12 @@ namespace d_fall_pps ...@@ -27,9 +38,12 @@ namespace d_fall_pps
// Loads the current context of the client // Loads the current context of the client
void loadCrazyflieContext(); void loadCrazyflieContext();
// loads Settings from UWBManagerService // Loads Settings from UWBManagerService
void loadUWBSettings(); void loadUWBSettings();
/************************************************************
* Callbacks *
***********************************************************/
// Callback function to handle incoming Vicon data // Callback function to handle incoming Vicon data
void viconDataCallback(const ViconData &viconData); void viconDataCallback(const ViconData &viconData);
...@@ -39,7 +53,7 @@ namespace d_fall_pps ...@@ -39,7 +53,7 @@ namespace d_fall_pps
// Callback function to handle incoming UWB data // Callback function to handle incoming UWB data
void UWBDataCallback(const CrazyflieData &data); void UWBDataCallback(const CrazyflieData &data);
// Callback function called when teachers changes UWB Status // Callback function called when student changes UWB Status
void uwbChangedCallback(const std_msgs::Int32 &msg); void uwbChangedCallback(const std_msgs::Int32 &msg);
} }
......
// Class to handle subscription to students to receive position data.
//
// Copyright (C) 2018
// Yvan Bosshard byvan @ee.ethz.ch
// Michael Rogenmoser michaero @ee.ethz.ch
// Tiago Salzmann tiagos @ee.ethz.ch
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef STUDENTSUBSCRIBER_H #ifndef STUDENTSUBSCRIBER_H
#define STUDENTSUBSCRIBER_H #define STUDENTSUBSCRIBER_H
...@@ -16,7 +36,7 @@ namespace d_fall_pps ...@@ -16,7 +36,7 @@ namespace d_fall_pps
void unsubscribe(); void unsubscribe();
// The callback once data arrives from this node // The callback once data arrives from this node
void callback(const CrazyflieData& data); void callback(const CrazyflieData& data); // make this private?
private: private:
int _studentID; int _studentID;
...@@ -26,4 +46,4 @@ namespace d_fall_pps ...@@ -26,4 +46,4 @@ namespace d_fall_pps
}; };
} }
#endif #endif // STUDENTSUBSCRIBER_H included
\ No newline at end of file \ No newline at end of file
// ROS node that handles subscription to and position data from
// students regardless of position data source used.
//
// Copyright (C) 2018
// Yvan Bosshard byvan @ee.ethz.ch
// Michael Rogenmoser michaero @ee.ethz.ch
// Tiago Salzmann tiagos @ee.ethz.ch
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef TEACHERSERVICE_H #ifndef TEACHERSERVICE_H
#define TEACHERSERVICE_H #define TEACHERSERVICE_H
#include "std_msgs/Int32.h" #include "std_msgs/Int32.h"
#include "d_fall_pps/CrazyflieData.h" #include "d_fall_pps/CrazyflieData.h"
#include "StudentSubscriber.h" #include "StudentSubscriber.h"
//#include "ros/ros.h"
namespace d_fall_pps namespace d_fall_pps
{ {
// Stores the connected student IDs // Stores the connected student IDs
std::vector<int> studentIDs; std::vector<int> studentIDs;
// and subscriptions // and subscriptions
std::vector<StudentSubscriber> studentSubscribers; 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 // Updates the subscription to the localization data of the students
void subscribeLocalization(); void subscribeLocalization();
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher; /************************************************************
* Callback *
***********************************************************/
// Callback once the refresh Student ID button is clicked
void refreshStudents_callback(const std_msgs::Int32 &msg);
} }
#endif // TEACHERSERVICE_H included #endif // TEACHERSERVICE_H included
// ROS node that can be used to log vicon and uwb data for analysis. // ROS node that can be used to log vicon and uwb data for analysis.
// Copyright (C) 2017 Michael Rogenmoser, Tiago Salzmann, Yvan Bosshard // Vicon data is cached until uwb data is updated and then logged.
//
// Copyright (C) 2018
// Yvan Bosshard byvan @ee.ethz.ch
// Michael Rogenmoser michaero @ee.ethz.ch
// Tiago Salzmann tiagos @ee.ethz.ch
// //
// This program is free software: you can redistribute it and/or modify // This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by // it under the terms of the GNU General Public License as published by
...@@ -35,20 +40,26 @@ namespace d_fall_pps ...@@ -35,20 +40,26 @@ namespace d_fall_pps
// Loads the current context of the client // Loads the current context of the client
void loadCrazyflieContext(); void loadCrazyflieContext();
// Callback function to handle the change in database information // Prepares logs
void dbChangedCallback(const std_msgs::Int32 &msg);
// prepares logs
void prepareLogs(); void prepareLogs();
// Writes data and timestamp into the passed file
void writeToLog(std::ofstream& file, long timestamp, const CrazyflieData& data); void writeToLog(std::ofstream& file, long timestamp, const CrazyflieData& data);
/************************************************************
* Callbacks *
***********************************************************/
// Callback function to handle the change in database information
void dbChangedCallback(const std_msgs::Int32 &msg);
// Callback function to handle incoming Vicon data // Callback function to handle incoming Vicon data
void viconDataCallback(const ViconData &viconData); void viconDataCallback(const ViconData &viconData);
// Callback function to handle incoming UWB data // Callback function to handle incoming UWB data
void UWBDataCallback(const CrazyflieData &data); void UWBDataCallback(const CrazyflieData &data);
// Callback function to handle updated UWB distances data
void DistancesCallback(const std_msgs::Float32MultiArray& distances); void DistancesCallback(const std_msgs::Float32MultiArray& distances);
} }
......
/* // This ROS node is subscribed to the ViconDataPublisher as well as
* // to the UWBDataPublisher. It is used to relay the data from the
* Localization Server // desired localisation source to the controller and the GUI.
* //
* This node is subscribed to the ViconDataPublisher as well // Copyright (C) 2018
* as to the UWBDataPublisher. It is used to relay the data // Yvan Bosshard byvan @ee.ethz.ch
* of the desired localization source to the PPSClient and // Michael Rogenmoser michaero @ee.ethz.ch
* can compare the data provided by both. // Tiago Salzmann tiagos @ee.ethz.ch
* //
*/ // This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include <chrono> #include <chrono>
#include <stdlib.h> #include <stdlib.h>
#include <std_msgs/String.h> #include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h> #include <ros/package.h>
#include "LocalizationServer.h" #include "LocalizationServer.h"
...@@ -34,7 +44,6 @@ ros::ServiceClient centralManager; ...@@ -34,7 +44,6 @@ ros::ServiceClient centralManager;
ros::ServiceClient uwbManager; ros::ServiceClient uwbManager;
ros::Publisher localizationPublisher; ros::Publisher localizationPublisher;
CrazyflieContext context; CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB = false; bool enableUWB = false;
bool useUWB = false; bool useUWB = false;
...@@ -43,6 +52,9 @@ bool useUWB = false; ...@@ -43,6 +52,9 @@ bool useUWB = false;
CrazyflieData CFData_vicon; CrazyflieData CFData_vicon;
CrazyflieData CFData_uwb; CrazyflieData CFData_uwb;
/************************************************************
* Main *
***********************************************************/
int main(int argc, char* argv[]) int main(int argc, char* argv[])
{ {
// Initialize the node and it's handles // Initialize the node and it's handles
...@@ -62,7 +74,6 @@ int main(int argc, char* argv[]) ...@@ -62,7 +74,6 @@ int main(int argc, char* argv[])
uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false); uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback); ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
//ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
loadUWBSettings(); loadUWBSettings();
// subscribe to the global vicon publisher and local uwb publisher // subscribe to the global vicon publisher and local uwb publisher
...@@ -71,24 +82,20 @@ int main(int argc, char* argv[]) ...@@ -71,24 +82,20 @@ int main(int argc, char* argv[])
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback); ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB"); ROS_INFO_STREAM("[LocalizationServer] subscribed to UWB");
//TODO: need to add a switch or something in a gui (teacher or student) with a publisher, to be subscribed here
// initialize the position data publisher // initialize the position data publisher
localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1); localizationPublisher = nodeHandle.advertise<CrazyflieData>("LocalizationData", 1);
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
std::string record_file = package_path + "log/LocalizationServerLog.bag";
bag.open(record_file, rosbag::bagmode::Write);
ROS_INFO_STREAM("[LocalizationServer] started!"); ROS_INFO_STREAM("[LocalizationServer] started!");
ros::spin(); ros::spin();
bag.close();
return 0; return 0;
} }
/************************************************************
* Functions *
***********************************************************/
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle) bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{ {
if(!nodeHandle.getParam("clientID", clientID)) if(!nodeHandle.getParam("clientID", clientID))
...@@ -134,22 +141,10 @@ void d_fall_pps::loadUWBSettings() ...@@ -134,22 +141,10 @@ void d_fall_pps::loadUWBSettings()
ROS_WARN("[LocalizationServer] Use of UWB deactivated!"); ROS_WARN("[LocalizationServer] Use of UWB deactivated!");
} }
void tiagosPrint(double x, double y, double z)
{
// Anchor2
//double anchorPos[3] = {1.89, 0.61, 1.6};
// Anchor5
double anchorPos[3] = {-0.76, -1.36, 0.6};
/*ROS_WARN("Dist to A5: %lf", sqrt(