From af138557e09939475d64ac72bb525a3f6a65cdec Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Tue, 5 Sep 2017 16:29:29 +0200
Subject: [PATCH] fixed some bug with setpoint in safe controller

---
 pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp | 2 +-
 pps_ws/src/d_fall_pps/param/SafeController.yaml            | 4 ++--
 pps_ws/src/d_fall_pps/src/CustomControllerService.cpp      | 2 +-
 3 files changed, 4 insertions(+), 4 deletions(-)

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 891e14df..f3de765d 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
@@ -46,7 +46,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
     DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
 
     ros::NodeHandle my_nodeHandle("~");
-    controllerSetpointPublisher = nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
+    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);
 
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index d660059e..62302025 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/pps_ws/src/d_fall_pps/param/SafeController.yaml
@@ -18,7 +18,7 @@ estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
 defaultSetpoint: [0.0, 0.0, 0.3, 0.0]
 
 #take off and landing parameters (in meters and seconds)
-takeOffDistance : 1
-landingDistance : 0.15
+takeOffDistance : 0.7
+landingDistance : 0.1
 durationTakeOff : 3
 durationLanding : 3
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index e1fd96f1..ab612385 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -93,7 +93,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     //Tip: create functions that you call here to keep you code cleaner
 
 
-
+    ROS_INFO("custom controller loop");
 
 
 
-- 
GitLab