diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index b7d297d03af39f9d536fa0e165466efe7e2013cc..ca47a90e86023e0e325567ee2cc369b726c113d3 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -98,7 +98,7 @@ private:
ros::Subscriber CFBatterySubscriber;
ros::Subscriber flyingStateSubscriber;
- ros::Publisher setpointPublisher;
+ ros::Publisher controllerSetpointPublisher;
ros::Subscriber setpointSubscriber;
ros::Subscriber DBChangedSubscriber;
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index 33ba1e8f3d2a883474f7f2588b429e407450a4db..2792f85b3270bb8c4fd73e893e48801007c58950 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -38,11 +38,12 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
- setpointPublisher = nodeHandle.advertise<Setpoint>("SafeControllerService/Setpoint", 1);
+
setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this);
DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
ros::NodeHandle my_nodeHandle("~");
+ controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
customYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("customYAMLloaded", 1);
safeYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("safeYAMLloaded", 1);
@@ -325,7 +326,7 @@ void MainWindow::on_set_setpoint_button_clicked()
msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
- this->setpointPublisher.publish(msg_setpoint);
+ this->controllerSetpointPublisher.publish(msg_setpoint);
}
void MainWindow::on_pushButton_3_clicked()
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index e0bf6fba14e8db8409a86368d3cfb3e170f14575..69c5a8d204fd1035aaa7401a587d40e3149da4df 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -326,6 +326,13 @@
</property>
</widget>
</item>
+ <item row="1" column="0">
+ <widget class="QPushButton" name="load_safe_yaml_button">
+ <property name="text">
+ <string>Load YAML file</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
<widget class="QWidget" name="tab_4">
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index 238b62d08ba7e85edd66b9d8518564fb535f2748..e212fe83d5e10ff93ab6a97bb62b133e29c2c64d 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -77,6 +77,8 @@ ros::ServiceClient customController;
bool strictSafety;
float angleMargin;
+Setpoint controller_setpoint;
+
ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;
@@ -260,25 +262,24 @@ void landTimerCallback(const ros::TimerEvent&)
finished_land = true;
}
-void goToDefaultSetpoint()
+void goToControllerSetpoint()
{
- std::vector<float> default_setpoint(4);
- ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
-
- ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
-
- if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
- {
- ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
- }
-
- Setpoint setpoint_msg;
- setpoint_msg.x = default_setpoint[0];
- setpoint_msg.y = default_setpoint[1];
- setpoint_msg.z = default_setpoint[2];
- ROS_INFO_STREAM("Z =" << default_setpoint[2]);
- setpoint_msg.yaw = default_setpoint[3];
- safeControllerServiceSetpointPublisher.publish(setpoint_msg);
+ // std::vector<float> default_setpoint(4);
+ // ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
+
+ // ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
+
+ // if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
+ // {
+ // ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
+ // }
+
+ // Setpoint setpoint_msg;
+ // setpoint_msg.y = default_setpoint[1];
+ // setpoint_msg.z = default_setpoint[2];
+ // ROS_INFO_STREAM("Z =" << default_setpoint[2]);
+ // setpoint_msg.yaw = default_setpoint[3];
+ safeControllerServiceSetpointPublisher.publish(controller_setpoint);
}
//is called when new data from Vicon arrives
@@ -323,8 +324,8 @@ void viconCallback(const ViconData& viconData) {
if(changed_state_flag) // stuff that will be run only once when changing state
{
changed_state_flag = false;
- // need to change setpoint to the one from file
- goToDefaultSetpoint();
+ // need to change setpoint to the controller one
+ goToControllerSetpoint();
ROS_INFO("STATE_FLYING");
}
break;
@@ -502,12 +503,41 @@ void crazyRadioStatusCallback(const std_msgs::Int32& msg)
crazyradio_status = msg.data;
}
+void controllerSetPointCallback(const Setpoint& newSetpoint)
+{
+ // load in variable the setpoint
+ controller_setpoint = newSetpoint;
+
+ // if we are in flying, set it up NOW
+ if(flying_state == STATE_FLYING)
+ {
+ goToControllerSetpoint();
+ }
+}
+
int main(int argc, char* argv[])
{
ros::init(argc, argv, "PPSClient");
ros::NodeHandle nodeHandle("~");
ros_namespace = ros::this_node::getNamespace();
+ // load default setpoint
+ std::vector<float> default_setpoint(4);
+ ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
+
+ ROS_INFO_STREAM(ros_namespace << "/SafeControllerService");
+
+ if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
+ {
+ ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
+ }
+
+ controller_setpoint.x = default_setpoint[0];
+ controller_setpoint.y = default_setpoint[1];
+ controller_setpoint.z = default_setpoint[2];
+ controller_setpoint.yaw = default_setpoint[3];
+
+ // load context parameters
loadParameters(nodeHandle);
//ros::service::waitForService("/CentralManagerService/CentralManager");
@@ -542,6 +572,7 @@ int main(int argc, char* argv[])
// SafeControllerServicePublisher:
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
+ ros::Subscriber controllerSetpointSubscriber = namespaceNodeHandle.subscribe("student_GUI/ControllerSetpoint", 1, controllerSetPointCallback);
// subscriber for DBChanged
ros::Subscriber DBChangedSubscriber = namespaceNodeHandle.subscribe("/my_GUI/DBChanged", 1, DBChangedCallback);