Commit 21e623d0 authored by roangel's avatar roangel
Browse files

First compiling version with ros library inside GUI code. Now need to do...

First compiling version with ros library inside GUI code. Now need to do something meaningful. MSG, shared folder with the rest of the team?
parent bea60875
......@@ -13,6 +13,8 @@ find_package(catkin REQUIRED COMPONENTS
std_msgs
)
# Add precompiler definitions to include ROS things in GUI compilation
add_definitions(-DCATKIN_MAKE)
set(CMAKE_INCLUDE_CURRENT_DIR ON)
set(CMAKE_AUTOMOC ON)
......@@ -185,6 +187,7 @@ add_library(my_library ${MY_LIB_SOURCES})
target_link_libraries(my_GUI Qt5::Widgets) # let my_GUI have acesss to Qt stuff
target_link_libraries(my_library Qt5::Widgets) # let my_library have access to Qt stuff
target_link_libraries(my_GUI my_library)
target_link_libraries(my_GUI ${catkin_LIBRARIES})
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
......
#ifndef MAINGUIWINDOW_H
#define MAINGUIWINDOW_H
#define DEBUG_GUI
// The #define CATKIN_MAKE comes from cmake file
#include <QMainWindow>
#include <QTimer>
#include <QGridLayout>
#include <QGraphicsRectItem>
#ifndef DEBUG_GUI
#ifdef CATKIN_MAKE
#include "ros/callback_queue.h"
#include "ros/ros.h"
#include "CrazyFlieTypes.h"
#endif
#include "ui_mainguiwindow.h"
#include "myGraphicsScene.h"
......@@ -22,7 +21,7 @@ class MainGUIWindow;
}
#ifndef DEBUG_GUI
#ifdef CATKIN_MAKE
struct setpoint
{
double x;
......@@ -59,22 +58,13 @@ class MainGUIWindow : public QMainWindow
Q_OBJECT
public:
#ifdef DEBUG_GUI
explicit MainGUIWindow(/*ros::NodeHandle* ,*/ /*ros::CallbackQueue* callbackQueue, ros::Publisher* publisherMotorCommandsGUI,*/ QWidget *parent = 0);
#else
explicit MainGUIWindow(ros::NodeHandle* , /*ros::CallbackQueue* callbackQueue, ros::Publisher* publisherMotorCommandsGUI,*/ QWidget *parent = 0);
#endif
explicit MainGUIWindow(QWidget *parent = 0);
~MainGUIWindow();
#ifndef DEBUG_GUI
void init();
#endif
public slots:
#ifndef DEBUG_GUI
void runCallbacks();
#endif
private slots:
#ifndef DEBUG_GUI
#ifdef CATKIN_MAKE
#endif
void set_tabs(int n);
......@@ -104,64 +94,10 @@ private:
Ui::MainGUIWindow *ui;
myGraphicsScene* scene;
QGraphicsRectItem* item1;
void _init();
#ifndef DEBUG_GUI
void readDefaultParameters();
ros::CallbackQueue m_CallbackQueue;
ros::NodeHandle* m_pNodeHandle;
// publishers
// ros::Publisher* m_pPublisherMotorCommandsGUI;
ros::Publisher* m_pPublisherControllerParam;
ros::Publisher* m_pPublisherPositionSetpoint;
ros::Publisher* m_pPublisherSampleTime;
ros::Publisher* m_pPublisherControllerType;
ros::Publisher* m_pPublisherDoSomething;
ros::Publisher* m_pPublisherFeedforwardCmd;
crazypkg::ControllerParam m_controllerParam;
crazypkg::PositionSetpoint m_positionSetpoint;
crazypkg::MotorCommands m_DummyCommands;
crazypkg::SampleTimeParam m_sampleTimeParam;
std_msgs::Int32 m_controllerType;
std_msgs::Int32 m_DoSomething;
crazypkg::MotorCommands m_feedforwardCmd;
//subscribers
ros::Subscriber* m_pSubscriberControllerOutput;
ros::Subscriber* m_pSubscriberViconData;
ros::Subscriber* m_pSubscriberCntViconDataMissed;
// crazypkg::MotorCommands m_MotorCommands;
// crazypkg::ViconData m_ViconData;
// params
// PIDParams m_PIDParams [countPIDControllers];
// default params
PIDParams m_DefaultPIDParams[countPIDControllers];
PIDParams m_DefaultRateParams[countRateControllers];
double m_DefaultSampleTime[countSampleTimeTypes];
crazypkg::MotorCommands m_DefaultFeedforwardCmd;
CSetpointQueue m_trajCircle;
CSetpointQueue m_trajSquare;
setpoint m_currSetpoint;
bool m_isStopButtonActive;
bool m_isCalActive;
enum ETrajectoryType
{
eTrajCustom,
eTrajCircle,
eTrajSquare
};
ETrajectoryType m_trajectoryType;
#ifdef CATKIN_MAKE
#endif
};
......
......@@ -2,35 +2,23 @@
#include "ui_mainguiwindow.h"
#include <QApplication>
#ifndef DEBUG_GUI
#include "CrazyFlieInclude.h"
#ifdef CATKIN_MAKE
#endif
int main(int argc, char *argv[])
{
#ifndef DEBUG_GUI
#ifdef CATKIN_MAKE
ros::init(argc, argv, "GUI");
ros::NodeHandle nodeHandle("~");
#endif
QApplication applicationGUI(argc, argv);
#ifndef DEBUG_GUI
MainGUIWindow mainWindow(&nodeHandle);
mainWindow.init();
#else
MainGUIWindow mainWindow;
#endif
#ifndef DEBUG_GUI
QTimer *timerExecuteCallbacks = new QTimer(&applicationGUI);
mainWindow.connect(timerExecuteCallbacks, SIGNAL(timeout()), &mainWindow, SLOT(runCallbacks()));
timerExecuteCallbacks->start(100);
#endif
mainWindow.show();
applicationGUI.exec();
#ifndef DEBUG_GUI
#ifdef CATKIN_MAKE
ROS_WARN("GUI application terminated");
#endif
}
......@@ -13,21 +13,11 @@
#define N_MAX_CRAZYFLIES 20 // protection number
#ifndef DEBUG_GUI
MainGUIWindow::MainGUIWindow(ros::NodeHandle* nodeHandle, /*ros::CallbackQueue *callbackQueue,
ros::Publisher* publisherMotorCommandsGUI,*/
QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainGUIWindow),
m_pNodeHandle(nodeHandle)
{
ui->setupUi(this);
m_isStopButtonActive=false;
m_isCalActive=false;
m_trajectoryType=eTrajCustom;
_init();
}
#ifdef CATKIN_MAKE
#else
#endif
MainGUIWindow::MainGUIWindow(QWidget *parent) :
QMainWindow(parent),
ui(new Ui::MainGUIWindow)
......@@ -36,7 +26,6 @@ MainGUIWindow::MainGUIWindow(QWidget *parent) :
ui->setupUi(this);
_init();
}
#endif
MainGUIWindow::~MainGUIWindow()
{
......@@ -73,197 +62,10 @@ void MainGUIWindow::_init()
QObject::connect(scene, SIGNAL(numTablePiecesChanged(int)), this, SLOT(handleTablePiecesNumChanged(int)));
}
#ifndef DEBUG_GUI
void MainGUIWindow::init()
{
m_pNodeHandle->setCallbackQueue(&m_CallbackQueue);
// m_pPublisherMotorCommandsGUI=new ros::Publisher(m_pNodeHandle->advertise
// <crazypkg::MotorCommands>("topicDummyControllerCmd", 1));
m_pPublisherControllerParam=new ros::Publisher(m_pNodeHandle->advertise
<crazypkg::ControllerParam>("topicControllerParam", 100));
m_pPublisherPositionSetpoint=new ros::Publisher(m_pNodeHandle->advertise
<crazypkg::PositionSetpoint>("topicPositionSetpoint", 1));
m_pPublisherSampleTime=new ros::Publisher(m_pNodeHandle->advertise
<crazypkg::SampleTimeParam>("topicSampleTimeParam", 20));
m_pPublisherControllerType=new ros::Publisher(m_pNodeHandle->advertise
<std_msgs::Int32>("topicControllerType", 1));
m_pPublisherDoSomething=new ros::Publisher(m_pNodeHandle->advertise
<std_msgs::Int32>("topicDoSomething", 20));
m_pPublisherFeedforwardCmd=new ros::Publisher(m_pNodeHandle->advertise
<crazypkg::MotorCommands>("topicFeedforwardCmd",1));
// m_pSubscriberControllerOutput=new ros::Subscriber(m_pNodeHandle->subscribe
// ("/FlightControl/topicControllerOutput",1,&MainGUIWindow::callbackControllerOutput,this));
// m_pSubscriberViconData=new ros::Subscriber(m_pNodeHandle->subscribe
// ("/ViconDataStreamSDK/topicViconData",1,&MainGUIWindow::callbackViconData,this));
// m_pSubscriberCntViconDataMissed=new ros::Subscriber(m_pNodeHandle->subscribe
// ("/FlightControl/topicCntViconDataMissed",1,&MainGUIWindow::callbackCntViconDataMissed,this));
// initPIDParamsTable();
// initRateParamsTable();
readDefaultParameters();
ros::Time::init();
ros::Duration(3).sleep();
m_CallbackQueue.callAvailable(ros::WallDuration(0));
ros::Duration(1).sleep();
}
#ifndef CATKIN_MAKE
void MainGUIWindow::runCallbacks()
{
m_CallbackQueue.callAvailable(ros::WallDuration(0));
// updateSetpoint();
}
void MainGUIWindow::readDefaultParameters()
{
m_pNodeHandle->param<double>("KpX",m_DefaultPIDParams[ePIDX].Kp,0);
m_pNodeHandle->param<double>("KiX",m_DefaultPIDParams[ePIDX].Ki,0);
m_pNodeHandle->param<double>("KdX",m_DefaultPIDParams[ePIDX].Kd,0);
m_pNodeHandle->param<double>("NX",m_DefaultPIDParams[ePIDX].N,60);
m_pNodeHandle->param<double>("MinSatPIDX",m_DefaultPIDParams[ePIDX].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatPIDX",m_DefaultPIDParams[ePIDX].MaxSat,98765);
m_pNodeHandle->param<double>("KpY",m_DefaultPIDParams[ePIDY].Kp,0);
m_pNodeHandle->param<double>("KiY",m_DefaultPIDParams[ePIDY].Ki,0);
m_pNodeHandle->param<double>("KdY",m_DefaultPIDParams[ePIDY].Kd,0);
m_pNodeHandle->param<double>("NY",m_DefaultPIDParams[ePIDY].N,60);
m_pNodeHandle->param<double>("MinSatPIDY",m_DefaultPIDParams[ePIDY].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatPIDY",m_DefaultPIDParams[ePIDY].MaxSat,98765);
m_pNodeHandle->param<double>("KpZ",m_DefaultPIDParams[ePIDZ].Kp,0);
m_pNodeHandle->param<double>("KiZ",m_DefaultPIDParams[ePIDZ].Ki,0);
m_pNodeHandle->param<double>("KdZ",m_DefaultPIDParams[ePIDZ].Kd,0);
m_pNodeHandle->param<double>("NZ",m_DefaultPIDParams[ePIDZ].N,60);
m_pNodeHandle->param<double>("MinSatPIDZ",m_DefaultPIDParams[ePIDZ].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatPIDZ",m_DefaultPIDParams[ePIDZ].MaxSat,98765);
m_pNodeHandle->param<double>("KpYaw",m_DefaultPIDParams[ePIDYaw].Kp,0);
m_pNodeHandle->param<double>("KiYaw",m_DefaultPIDParams[ePIDYaw].Ki,0);
m_pNodeHandle->param<double>("KdYaw",m_DefaultPIDParams[ePIDYaw].Kd,0);
m_pNodeHandle->param<double>("NYaw",m_DefaultPIDParams[ePIDYaw].N,60);
m_pNodeHandle->param<double>("MinSatPIDYaw",m_DefaultPIDParams[ePIDYaw].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatPIDYaw",m_DefaultPIDParams[ePIDYaw].MaxSat,98765);
m_pNodeHandle->param<double>("KpPitch",m_DefaultPIDParams[ePIDPitch].Kp,0);
m_pNodeHandle->param<double>("KiPitch",m_DefaultPIDParams[ePIDPitch].Ki,0);
m_pNodeHandle->param<double>("KdPitch",m_DefaultPIDParams[ePIDPitch].Kd,0);
m_pNodeHandle->param<double>("NPitch",m_DefaultPIDParams[ePIDPitch].N,60);
m_pNodeHandle->param<double>("MinSatPIDPitch",m_DefaultPIDParams[ePIDPitch].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatPIDPitch",m_DefaultPIDParams[ePIDPitch].MaxSat,98765);
m_pNodeHandle->param<double>("KpRoll",m_DefaultPIDParams[ePIDRoll].Kp,0);
m_pNodeHandle->param<double>("KiRoll",m_DefaultPIDParams[ePIDRoll].Ki,0);
m_pNodeHandle->param<double>("KdRoll",m_DefaultPIDParams[ePIDRoll].Kd,0);
m_pNodeHandle->param<double>("NRoll",m_DefaultPIDParams[ePIDRoll].N,60);
m_pNodeHandle->param<double>("MinSatPIDRoll",m_DefaultPIDParams[ePIDRoll].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatPIDRoll",m_DefaultPIDParams[ePIDRoll].MaxSat,98765);
m_pNodeHandle->param<double>("KpRateYaw",m_DefaultRateParams[ePIDYawRate].Kp,0);
m_pNodeHandle->param<double>("KiRateYaw",m_DefaultRateParams[ePIDYawRate].Ki,0);
m_pNodeHandle->param<double>("KdRateYaw",m_DefaultRateParams[ePIDYawRate].Kd,0);
m_pNodeHandle->param<double>("NRateYaw",m_DefaultRateParams[ePIDYawRate].N,60);
m_pNodeHandle->param<double>("MinSatRateYaw",m_DefaultRateParams[ePIDYawRate].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatRateYaw",m_DefaultRateParams[ePIDYawRate].MaxSat,98765);
m_pNodeHandle->param<double>("KpRatePitch",m_DefaultRateParams[ePIDPitchRate].Kp,0);
m_pNodeHandle->param<double>("KiRatePitch",m_DefaultRateParams[ePIDPitchRate].Ki,0);
m_pNodeHandle->param<double>("KdRatePitch",m_DefaultRateParams[ePIDPitchRate].Kd,0);
m_pNodeHandle->param<double>("NRatePitch",m_DefaultRateParams[ePIDPitchRate].N,60);
m_pNodeHandle->param<double>("MinSatRatePitch",m_DefaultRateParams[ePIDPitchRate].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatRatePitch",m_DefaultRateParams[ePIDPitchRate].MaxSat,98765);
m_pNodeHandle->param<double>("KpRateRoll",m_DefaultRateParams[ePIDRollRate].Kp,0);
m_pNodeHandle->param<double>("KiRateRoll",m_DefaultRateParams[ePIDRollRate].Ki,0);
m_pNodeHandle->param<double>("KdRateRoll",m_DefaultRateParams[ePIDRollRate].Kd,0);
m_pNodeHandle->param<double>("NRateRoll",m_DefaultRateParams[ePIDRollRate].N,60);
m_pNodeHandle->param<double>("MinSatRateRoll",m_DefaultRateParams[ePIDRollRate].MinSat,-9876);
m_pNodeHandle->param<double>("MaxSatRateRoll",m_DefaultRateParams[ePIDRollRate].MaxSat,98765);
m_pNodeHandle->param<double>("SampleTimePID",m_DefaultSampleTime[ePIDTs],0.020);
m_pNodeHandle->param<double>("SampleTimeLQRFull",m_DefaultSampleTime[eLQRFullTs],0.020);
m_pNodeHandle->param<double>("SampleTimeLQRNested",m_DefaultSampleTime[eLQRNestedTs],0.020);
m_pNodeHandle->param<double>("SampleTimeRate",m_DefaultSampleTime[eRateTs],0.020);
m_pNodeHandle->param<float>("FeedforwardMotor1",m_DefaultFeedforwardCmd.cmd1,0.0);
m_pNodeHandle->param<float>("FeedforwardMotor2",m_DefaultFeedforwardCmd.cmd2,0.0);
m_pNodeHandle->param<float>("FeedforwardMotor3",m_DefaultFeedforwardCmd.cmd3,0.0);
m_pNodeHandle->param<float>("FeedforwardMotor4",m_DefaultFeedforwardCmd.cmd4,0.0);
}
CSetpointQueue::CSetpointQueue()
{
startElem=NULL;
currElem=NULL;
lastElem=NULL;
}
void CSetpointQueue::insert(setpoint newElem)
{
if (startElem==NULL)
{
startElem=new QueueElem(newElem);
lastElem=startElem;
currElem=startElem;
}
else
{
lastElem->next=new QueueElem(newElem);
lastElem=lastElem->next;
}
}
setpoint CSetpointQueue::getNext()
{
setpoint ret;
ret.x=currElem->elem.x;
ret.y=currElem->elem.y;
ret.z=currElem->elem.z;
ret.yaw=currElem->elem.yaw;
if(currElem->next!=NULL)
currElem=currElem->next;
else currElem=startElem;
return ret;
}
void CSetpointQueue::print()
{
QueueElem* p=startElem;
ROS_INFO_STREAM("queue elements: ");
int cnt=0;
while (p!=NULL)
{
cnt++;
ROS_INFO_STREAM("element "<<cnt<<": "<<"x="<<p->elem.x<<" y="<<p->elem.y<<" z="<<p->elem.z<<" yaw="<<p->elem.yaw);
p=p->next;
}
}
#endif // DEBUG_GUI
#endif
......@@ -340,16 +142,7 @@ void MainGUIWindow::on_radioButton_crazyfly_zones_mode_toggled(bool checked)
void MainGUIWindow::handleTablePiecesNumChanged(int newNum)
{
// if(newNum == 0)
// {
// ui->radioButton_crazyfly_zones_mode->setCheckable(false);
// ui->radioButton_crazyfly_zones_mode->setEnabled(false);
// }
// else
// {
// ui->radioButton_crazyfly_zones_mode->setCheckable(true);
// ui->radioButton_crazyfly_zones_mode->setEnabled(true);
// }
}
void MainGUIWindow::on_radioButton_lock_mode_toggled(bool checked)
......
Markdown is supported
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