Commit 3bc030c4 authored by muelmarc's avatar muelmarc
Browse files

merging

parent 4cae4cb6
......@@ -11,13 +11,13 @@
#include <ros/ros.h>
#include <ros/network.h>
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
#include "d_fall_pps/UnlabeledMarkersArray.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/ViconData.h"
using namespace d_fall_pps;
typedef UnlabeledMarkersArray::ConstPtr ptrToMessage;
typedef ViconData::ConstPtr ptrToMessage;
Q_DECLARE_METATYPE(ptrToMessage)
......@@ -31,12 +31,10 @@ public:
bool init();
// void messageCallback(const ViconData& data);
void messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg);
void messageCallback(const ptrToMessage& p_msg);
signals:
// void newViconData(double, double, double, double, double, double);
// void newViconData(double, double);
void newViconData(const ptrToMessage& p_msg);
......
......@@ -25,7 +25,7 @@ MainGUIWindow::MainGUIWindow(int argc, char **argv, QWidget *parent) :
ui(new Ui::MainGUIWindow)//,
// _rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData")
{
_rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/UnlabeledMarkersArray");
_rosNodeThread = new rosNodeThread(argc, argv, "/ViconDataPublisher/ViconData");
ui->setupUi(this);
_init();
......
......@@ -44,7 +44,7 @@ bool rosNodeThread::init()
return true;
} // set up the thread
void rosNodeThread::messageCallback(const UnlabeledMarkersArray::ConstPtr& p_msg) // When a message arrives to the topic, this callback is executed
void rosNodeThread::messageCallback(const ptrToMessage& p_msg) // When a message arrives to the topic, this callback is executed
{
emit newViconData(p_msg); //pass the message to other places
}
......
//for field command in CMCommand
#define CMD_SAVE 1
#define CMD_RELOAD 2
//for field mode in CMUpdate
#define ENTRY_INSERT_OR_UPDATE 1
#define ENTRY_REMOVE 2
<launch>
<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
</node>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
......@@ -25,7 +25,10 @@
<node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" />
<!-- When we have a GUI, this has to be adapted and included -->
<node pkg="rosbag" type="record" name="testbag" />
<!-- 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">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node>
......
......@@ -16,6 +16,8 @@
#include <stdlib.h>
#include "ros/ros.h"
#include <ros/package.h>
#include "rosbag/bag.h"
#include "d_fall_pps/CentralManager.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieDB.h"
......@@ -31,6 +33,18 @@ using namespace d_fall_pps;
using namespace std;
CrazyflieDB crazyflieDB;
rosbag::Bag bag;
void saveCrazyflieDB() {
string packagePath = ros::package::getPath("d_fall_pps") + "/";
string dbFile = packagePath + "crazyflie.db";
bag.open(dbFile, rosbag::bagmode::Write);
bag.write("crazyflie_db", ros::Time::now(), crazyflieDB);
}
void loadCrazyflieDB() {
}
bool cmRead(CMRead::Request &request, CMRead::Response &response) {
response.crazyflieDB = crazyflieDB;
......@@ -99,12 +113,12 @@ bool cmUpdate(CMUpdate::Request &request, CMUpdate::Response &response) {
bool cmCommand(CMCommand::Request &request, CMCommand::Response &response) {
switch(request.command) {
case CMD_SAVE: {
//writeCrazyflieDB(crazyflieDB);
saveCrazyflieDB();
return true;
}
case CMD_RELOAD: {
//crazyflieDB = readCrazyflieDB();
loadCrazyflieDB();
return true;
}
......
......@@ -20,7 +20,7 @@
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
// #define TESTING_FAKE_DATA
#define TESTING_FAKE_DATA
// notice that unit here are in milimeters
using namespace ViconDataStreamSDK::CPP;
......@@ -48,22 +48,23 @@ int main(int argc, char* argv[]) {
}
// Testing piece of code
ViconData viconData;
UnlabeledMarker marker;
UnlabeledMarkersArray markersArray;
marker.index = 0;
marker.x = f;
marker.y = 0;
marker.z = 0;
markersArray.markers.push_back(marker);
viconData.markers.push_back(marker);
marker.index = 1;
marker.x = 0;
marker.y = f;
marker.z = 0;
markersArray.markers.push_back(marker);
viconData.markers.push_back(marker);
if(i > 50 && i < 100)
{
......@@ -71,13 +72,15 @@ int main(int argc, char* argv[]) {
marker.x = f;
marker.y = f;
marker.z = 0;
markersArray.markers.push_back(marker);
viconData.markers.push_back(marker);
}
unlabeledMarkersPublisher.publish(markersArray);
ros::Duration(0.1).sleep();
f += 10;
i++;
// TODO: Fake CF data
viconDataPublisher.publish(viconData); // testing data
}
#else
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment