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 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
#define UWBMARKER_H
......@@ -8,12 +30,12 @@
#include "d_fall_pps/UWBAnchor.h"
using namespace d_fall_pps;
#define ANCHOR_HEIGHT 200 * FROM_MILIMETERS_TO_UNITS * 1.2
#define ANCHOR_WIDTH 200 * FROM_MILIMETERS_TO_UNITS * 1.2
#define ANCHOR_OPACITY 0.7
using namespace d_fall_pps;
class UWBMarker : public QGraphicsSvgItem
{
public:
......
......@@ -23,8 +23,10 @@
#include <QGridLayout>
#include <QGraphicsRectItem>
#ifdef CATKIN_MAKE
#include <std_msgs/Int32.h>
#include <std_msgs/Int32MultiArray.h>
#include "rosNodeThread.h"
#include "marker.h"
#include "crazyFly.h"
......@@ -35,10 +37,6 @@
#include "d_fall_pps/CrazyflieEntry.h"
#include "d_fall_pps/Anchors.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Int32MultiArray.h>
#define CMD_CRAZYFLY_MOTORS_OFF 5
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 "UWBMarker.h"
UWBMarker::UWBMarker(const UWBAnchor* const anchor, QString filename, QGraphicsItem* parent) : QGraphicsSvgItem(filename)
{
updateMarker(anchor);
......@@ -8,8 +31,7 @@ UWBMarker::UWBMarker(const UWBAnchor* const anchor, QString filename, QGraphicsI
m_height = ANCHOR_HEIGHT;
}
UWBMarker::~UWBMarker()
{}
UWBMarker::~UWBMarker(){}
void UWBMarker::updateMarker(const UWBAnchor* const anchor)
{
......@@ -23,7 +45,6 @@ void UWBMarker::updateMarker(const UWBAnchor* const anchor)
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);
}
......
// Teacher's GUI main window header.
// Copyright (C) 2017 Angel Romero
// Teacher's GUI main window.
//
// 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
// it under the terms of the GNU General Public License as published by
......@@ -14,12 +20,8 @@
// 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 "mainguiwindow.h"
#include "ui_mainguiwindow.h"
#include "crazyFlyZoneTab.h"
#include "myGraphicsScene.h"
#include "myGraphicsView.h"
#include "channelLUT.h"
#include <regex>
#include <string>
#include <QObject>
#include <QDoubleSpinBox>
......@@ -28,9 +30,19 @@
#include <QMetaType>
#include <QDir>
#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
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/CMRead.h"
#include "d_fall_pps/CrazyflieEntry.h"
......@@ -38,16 +50,8 @@
#include "d_fall_pps/CMCommand.h"
#include "CentralManagerService.h"
#include <ros/ros.h>
#include <ros/network.h>
#include <ros/package.h>
#endif
//#include <iomanip>
#include <string>
#define N_MAX_CRAZYFLIES 20 // protection number
#define UWB_UPDATE_DISABLE 0
......
/*
*
* Localization Server
*
* This node is subscribed to the ViconDataPublisher as well
* as to the UWBDataPublisher. It is used to compare the
* data and relay the desired localization source to the
* PPSClient.
*
*/
// This ROS node is subscribed to the ViconDataPublisher as well as
// to the UWBDataPublisher. It is used to relay the data from the
// desired localisation source to the controller and the GUI.
//
// 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 LOCALIZATIONSERVER_H
#define LOCALIZATIONSERVER_H
......@@ -27,9 +38,12 @@ namespace d_fall_pps
// Loads the current context of the client
void loadCrazyflieContext();
// loads Settings from UWBManagerService
// Loads Settings from UWBManagerService
void loadUWBSettings();
/************************************************************
* Callbacks *
***********************************************************/
// Callback function to handle incoming Vicon data
void viconDataCallback(const ViconData &viconData);
......@@ -39,7 +53,7 @@ namespace d_fall_pps
// Callback function to handle incoming UWB 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);
}
......
// 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
#define STUDENTSUBSCRIBER_H
......@@ -16,7 +36,7 @@ namespace d_fall_pps
void unsubscribe();
// The callback once data arrives from this node
void callback(const CrazyflieData& data);
void callback(const CrazyflieData& data); // make this private?
private:
int _studentID;
......@@ -26,4 +46,4 @@ namespace d_fall_pps
};
}
#endif
\ No newline at end of file
#endif // STUDENTSUBSCRIBER_H included
\ 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
#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;
/************************************************************
* Callback *
***********************************************************/
// Callback once the refresh Student ID button is clicked
void refreshStudents_callback(const std_msgs::Int32 &msg);
}
#endif // TEACHERSERVICE_H included
// 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
// it under the terms of the GNU General Public License as published by
......@@ -35,20 +40,26 @@ namespace d_fall_pps
// Loads the current context of the client
void loadCrazyflieContext();
// Callback function to handle the change in database information
void dbChangedCallback(const std_msgs::Int32 &msg);
// prepares logs
// Prepares logs
void prepareLogs();
// Writes data and timestamp into the passed file
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
void viconDataCallback(const ViconData &viconData);
// Callback function to handle incoming UWB data
void UWBDataCallback(const CrazyflieData &data);
// Callback function to handle updated UWB distances data
void DistancesCallback(const std_msgs::Float32MultiArray& distances);
}
......
/*
*
* Localization Server
*
* This node is subscribed to the ViconDataPublisher as well
* as to the UWBDataPublisher. It is used to relay the data
* of the desired localization source to the PPSClient and
* can compare the data provided by both.
*
*/
// This ROS node is subscribed to the ViconDataPublisher as well as
// to the UWBDataPublisher. It is used to relay the data from the
// desired localisation source to the controller and the GUI.
//
// 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/>.
#include <chrono>
#include <stdlib.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>
#include "LocalizationServer.h"
......@@ -34,7 +44,6 @@ ros::ServiceClient centralManager;
ros::ServiceClient uwbManager;
ros::Publisher localizationPublisher;
CrazyflieContext context;
rosbag::Bag bag;
bool enableUWB = false;
bool useUWB = false;
......@@ -43,6 +52,9 @@ bool useUWB = false;
CrazyflieData CFData_vicon;
CrazyflieData CFData_uwb;
/************************************************************
* Main *
***********************************************************/
int main(int argc, char* argv[])
{
// Initialize the node and it's handles
......@@ -62,7 +74,6 @@ int main(int argc, char* argv[])
uwbManager = nodeHandle.serviceClient<Anchors>("/UWBManagerService/UWBData", false);
ros::Subscriber enableUWBSubscriber = namespaceNodeHandle.subscribe("student_GUI/uwbChanged", 1, uwbChangedCallback);
//ros::Subscriber enableUWBSubscriber = nodeHandle.subscribe("/my_GUI/enableUWB", 1, uwbChangedCallback);
loadUWBSettings();
// subscribe to the global vicon publisher and local uwb publisher
......@@ -71,24 +82,20 @@ int main(int argc, char* argv[])
ros::Subscriber UWBSubscriber = namespaceNodeHandle.subscribe("UWBDataPublisher/UWBData", 100, UWBDataCallback);
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
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::spin();
bag.close();
return 0;
}
/************************************************************
* Functions *
***********************************************************/
bool d_fall_pps::loadParameters(ros::NodeHandle &nodeHandle)
{
if(!nodeHandle.getParam("clientID", clientID))
......@@ -134,22 +141,10 @@ void d_fall_pps::loadUWBSettings()
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(
(x - anchorPos[0])*(x - anchorPos[0])
+ (y - anchorPos[1])*(y - anchorPos[1])
+ (z - anchorPos[2])*(z - anchorPos[2])
)
);*/
}
/************************************************************
* Callbacks *
***********************************************************/
void d_fall_pps::viconDataCallback(const ViconData &viconData)
{
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it)
......@@ -158,29 +153,30 @@ void d_fall_pps::viconDataCallback(const ViconData &viconData)
if(data.crazyflieName == context.crazyflieName)
{
// if desired, publish Vicon data
if(! (enableUWB && useUWB))
localizationPublisher.publish(data);
CFData_vicon = data;
bag.write("ViconData", ros::Time::now(), data);
// leave the for-loop so not every vector element has to be compared, once the matching context is found
break;
}
}
}
tiagosPrint(CFData_vicon.x, CFData_vicon.y, CFData_vicon.z);
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
d_fall_pps::loadCrazyflieContext();
}
// Callback function to handle incoming UWB data
void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
{
// override UWB Data
CFData_uwb = data;
//ROS_WARN("Roll: \tV: %f\tU: %f", CFData_vicon.roll, data.roll);
//ROS_WARN("E(R): V-U = %f", (CFData_vicon.roll - data.roll) * 180/3.14159);
// For the sake of nostalgia, the following lines
// were kept in the code
// Uncomment the lines to be overriden by vicon when using UWB
//CFData_uwb.x = CFData_vicon.x;
......@@ -188,20 +184,14 @@ void d_fall_pps::UWBDataCallback(const CrazyflieData &data)
//CFData_uwb.z = CFData_vicon.z;
//CFData_uwb.roll = CFData_vicon.roll;
//CFData_uwb.pitch = CFData_vicon.pitch;
//CFData_uwb.pitch *= -1;
//CFData_uwb.pitch *= -1; // (deprecated)
//CFData_uwb.yaw = CFData_vicon.yaw;
// if desired, publish UWB data
if(enableUWB && useUWB)
{
localizationPublisher.publish(CFData_uwb);
}
bag.write("UWBData", ros::Time::now(), CFData_uwb);
}
void d_fall_pps::dbChangedCallback(const std_msgs::Int32 &msg)
{
d_fall_pps::loadCrazyflieContext();
}
void d_fall_pps::uwbChangedCallback(const std_msgs::Int32 &msg)
......
// 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/>.
#include <regex>
#include "ros/ros.h"
#include "std_msgs/Int32MultiArray.h"
#include "d_fall_pps/CrazyfliePositionData.h"
#include "TeacherService.h"
using namespace d_fall_pps;
ros::Publisher studentIDs_publisher;
ros::Publisher CFData_publisher;
/************************************************************
* Main *
***********************************************************/
int main(int argc, char* argv[])
{
ros::init(argc, argv, "TeacherService");
......@@ -25,6 +53,43 @@ int main(int argc, char* argv[])
return 0;
}
/************************************************************
* Function *
***********************************************************/
void d_fall_pps::subscribeLocalization()
{
ros::NodeHandle nodeHandle("~");
for(std::vector<StudentSubscriber>::iterator it = studentSubscribers.begin(); it != studentSubscribers.end(); ++it)
(*it).unsubscribe();
studentSubscribers.clear();
for(std::vector<int>::const_iterator it = studentIDs.begin(); it != studentIDs.end(); ++it)
{
ROS_ERROR("Adding %i", (*it));
std::string address = "/";
address.append(std::to_string(*it));
address.append("/LocalizationServer/LocalizationData");
StudentSubscriber sub = StudentSubscriber(*it, address);
studentSubscribers.push_back(sub);