custom controller a bit more ready, still need to fill information in GUI...

custom controller a bit more ready, still need to fill information in GUI fields and connect everything to GUI (set setpoint mainly). Landing and take off parameters now in YAML file

pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp

@@ -43,7 +43,7 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
     DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
 
     ros::NodeHandle my_nodeHandle("~");
-    controllerSetpointPublisher = my_nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
+    controllerSetpointPublisher = nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
     customYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("customYAMLloaded", 1);
     safeYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("safeYAMLloaded", 1);
 

pps_ws/src/d_fall_pps/launch/Student.launch

@@ -14,6 +14,7 @@
 	</node>
 
 	<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
+		<rosparam command="load" file="$(find d_fall_pps)/param/CustomController.yaml" />
 	</node>
 
     <!-- When we have a GUI, this has to be adapted and included -->

pps_ws/src/d_fall_pps/param/ClientConfig.yaml

 safeController: "SafeControllerService/RateController"
-customController: "CircleControllerService/CircleController"
+customController: "CustomControllerService/CustomController"
 strictSafety: true
 angleMargin: 0.6
 

pps_ws/src/d_fall_pps/param/SafeController.yaml

@@ -16,3 +16,9 @@ estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
 
 #setpoint in meters (x, y, z, yaw)
 defaultSetpoint: [0.0, 0.0, 0.3, 0.0]
+
+#take off and landing parameters (in meters and seconds)
+takeOffDistance : 1
+landingDistance : 0.15
+durationTakeOff : 3
+durationLanding : 3

pps_ws/src/d_fall_pps/src/CustomControllerService.cpp

 // Place for students to implement their controller
-// The ROS::service is set to work    
+// The ROS::service is set to work
 
 
 //some useful libraries
@@ -13,6 +13,8 @@
 #include "d_fall_pps/ControlCommand.h"
 #include "d_fall_pps/Controller.h"
 
+#include <std_msgs/Int32.h>
+
 //constants
 #define PI 3.1415926535
 #define RATE_MODE 0
@@ -29,6 +31,22 @@ const float SATURATION_THRUST = MOTOR_REGRESSION_POLYNOMIAL[2] * 12000 * 12000 +
 using namespace d_fall_pps;
 
 
+// load parameters from corresponding YAML file
+
+void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
+    if(!nodeHandle.getParam(name, val)){
+        ROS_ERROR_STREAM("missing parameter '" << name << "'");
+    }
+    if(val.size() != length) {
+        ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
+    }
+}
+
+void loadCustomParameters(ros::NodeHandle& nodeHandle)
+{
+    // here we load the parameters that are in the CustomController.yaml
+}
+
 
 // ********* important function that has to be used!! *********
 //-est- is an array with the estimated values : x,y,z,vx,vy,vz,roll,pitch,yaw
@@ -56,7 +74,7 @@ void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured) {
 
 /* --- the data students can work with ---
     -request- contains data provided by Vicon. Check d_fall_pps/msg/ViconData.msg what it includes.
-    -response- is where you have to write your calculated data into. 
+    -response- is where you have to write your calculated data into.
 */
 bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
     //writing the data from -request- to command line
@@ -71,7 +89,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 
 
 
-    // ********* do your calculations here ********* 
+    // ********* do your calculations here *********
     //Tip: create functions that you call here to keep you code cleaner
 
 
@@ -94,18 +112,28 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
     it can either be Motor, Rate or Angle based */
     response.controlOutput.onboardControllerType = MOTOR_MODE;
     //response.controlOutput.onboardControllerType = RATE_MODE;
-    //response.controlOutput.onboardControllerType = ANGLE_MODE;    
-    
+    //response.controlOutput.onboardControllerType = ANGLE_MODE;
+
 	return true;
 }
 
+void customYAMLloadedCallback(const std_msgs::Int32& msg)
+{
+    ros::NodeHandle nodeHandle("~");
+    ROS_INFO("received msg custom loaded YAML");
+    loadCustomParameters(nodeHandle);
+}
+
 
 int main(int argc, char* argv[]) {
     //starting the ROS-node
     ros::init(argc, argv, "CustomControllerService");
     ros::NodeHandle nodeHandle("~");
-
     ros::ServiceServer service = nodeHandle.advertiseService("CustomController", calculateControlOutput);
+
+    ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
+    ros::Subscriber customYAMLloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/customYAMLloaded", 1, customYAMLloadedCallback);
+
     ROS_INFO("CustomControllerService ready");
     ros::spin();
 

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -111,6 +111,11 @@ bool usingSafeController;
 
 std::string ros_namespace;
 
+float take_off_distance;
+float landing_distance;
+float duration_take_off;
+float duration_landing;
+
 
 void loadSafeController() {
 	ros::NodeHandle nodeHandle("~");
@@ -196,7 +201,7 @@ void takeOffCF(CrazyflieData& current_local_coordinates) //local because the set
     Setpoint setpoint_msg;
     setpoint_msg.x = current_local_coordinates.x;           // previous one
     setpoint_msg.y = current_local_coordinates.y;           //previous one
-    setpoint_msg.z = current_local_coordinates.z + TAKE_OFF_OFFSET;           //previous one plus some offset
+    setpoint_msg.z = current_local_coordinates.z + take_off_distance;           //previous one plus some offset
     // setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
     setpoint_msg.yaw = 0.0;
     safeControllerServiceSetpointPublisher.publish(setpoint_msg);
@@ -218,7 +223,7 @@ void landCF(CrazyflieData& current_local_coordinates)
     Setpoint setpoint_msg;
     setpoint_msg.x = current_local_coordinates.x;           // previous one
     setpoint_msg.y = current_local_coordinates.y;           //previous one
-    setpoint_msg.z = LANDING_DISTANCE;           //previous one plus some offset
+    setpoint_msg.z = landing_distance;           //previous one plus some offset
     setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
     safeControllerServiceSetpointPublisher.publish(setpoint_msg);
 
@@ -312,7 +317,7 @@ void viconCallback(const ViconData& viconData) {
                         takeOffCF(local);
                         finished_take_off = false;
                         ROS_INFO("STATE_TAKE_OFF");
-                        timer_takeoff = nodeHandle.createTimer(ros::Duration(DURATION_TAKE_OFF), takeOffTimerCallback, true);
+                        timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_take_off), takeOffTimerCallback, true);
                     }
                     if(finished_take_off)
                     {
@@ -336,7 +341,7 @@ void viconCallback(const ViconData& viconData) {
                         landCF(local);
                         finished_land = false;
                         ROS_INFO("STATE_LAND");
-                        timer_takeoff = nodeHandle.createTimer(ros::Duration(DURATION_LANDING), landTimerCallback, true);
+                        timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_landing), landTimerCallback, true);
                     }
                     if(finished_land)
                     {
@@ -409,8 +414,30 @@ void loadParameters(ros::NodeHandle& nodeHandle) {
 		ROS_ERROR("Failed to get angleMargin param");
 		return;
 	}
+}
+
+void loadSafeControllerParameters()
+{
+    ros::NodeHandle nh_safeControllerService(ros_namespace + "/SafeControllerService");
+    if(!nh_safeControllerService.getParam("takeOffDistance", take_off_distance))
+    {
+		ROS_ERROR("Failed to get takeOffDistance");
+	}
 
+    if(!nh_safeControllerService.getParam("landingDistance", landing_distance))
+    {
+		ROS_ERROR("Failed to get landing_distance");
+	}
 
+    if(!nh_safeControllerService.getParam("durationTakeOff", duration_take_off))
+    {
+		ROS_ERROR("Failed to get duration_take_off");
+	}
+
+    if(!nh_safeControllerService.getParam("durationLanding", duration_landing))
+    {
+		ROS_ERROR("Failed to get duration_landing");
+	}
 }
 
 void loadCrazyflieContext() {
@@ -515,6 +542,13 @@ void controllerSetPointCallback(const Setpoint& newSetpoint)
     }
 }
 
+
+void safeYAMLloadedCallback(const std_msgs::Int32& msg)
+{
+    ROS_INFO("received msg safe loaded YAML");
+    loadSafeControllerParameters();
+}
+
 int main(int argc, char* argv[])
 {
 	ros::init(argc, argv, "PPSClient");
@@ -539,6 +573,7 @@ int main(int argc, char* argv[])
 
     // load context parameters
 	loadParameters(nodeHandle);
+    loadSafeControllerParameters();
 
 	//ros::service::waitForService("/CentralManagerService/CentralManager");
 	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
@@ -580,6 +615,8 @@ int main(int argc, char* argv[])
     // crazyradio status. Connected, connecting or disconnected
     ros::Subscriber crazyRadioStatusSubscriber = namespaceNodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, crazyRadioStatusCallback);
 
+    ros::Subscriber safeYAMloadedSubscriber = namespaceNodeHandle.subscribe("student_GUI/safeYAMLloaded", 1, safeYAMLloadedCallback);
+
 	//start with safe controller
     flying_state = STATE_MOTORS_OFF;
 	usingSafeController = true;

pps_ws/src/d_fall_pps/src/SafeControllerService.cpp

@@ -267,13 +267,6 @@ void setpointCallback(const Setpoint& newSetpoint) {
     setpoint[3] = newSetpoint.yaw;
 }
 
-void customYAMLloadedCallback(const std_msgs::Int32& msg)
-{
-    ros::NodeHandle nodeHandle("~");
-    ROS_INFO("received msg custom loaded YAML");
-    // loadSafeParameters(nodeHandle);
-}
-
 void safeYAMLloadedCallback(const std_msgs::Int32& msg)
 {
     ros::NodeHandle nodeHandle("~");
@@ -296,7 +289,6 @@ int main(int argc, char* argv[]) {
 
     ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
 
-    ros::Subscriber customYAMloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/customYAMLloaded", 1, customYAMLloadedCallback);
     ros::Subscriber safeYAMloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/safeYAMLloaded", 1, safeYAMLloadedCallback);
 
     ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);