From b5aa168ffebda96ced24ef95e235a5aeaaa19818 Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Mon, 4 Sep 2017 18:08:38 +0200
Subject: [PATCH] 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
---
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 2 +-
pps_ws/src/d_fall_pps/launch/Student.launch | 1 +
pps_ws/src/d_fall_pps/param/ClientConfig.yaml | 2 +-
.../src/d_fall_pps/param/SafeController.yaml | 6 +++
.../src/CustomControllerService.cpp | 40 ++++++++++++++---
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 45 +++++++++++++++++--
.../d_fall_pps/src/SafeControllerService.cpp | 8 ----
7 files changed, 84 insertions(+), 20 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 2792f85b..2e1678e7 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
@@ -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);
diff --git a/pps_ws/src/d_fall_pps/launch/Student.launch b/pps_ws/src/d_fall_pps/launch/Student.launch
index 475bf792..c2480b38 100755
--- a/pps_ws/src/d_fall_pps/launch/Student.launch
+++ b/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 -->
diff --git a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
index 27a48ab3..4e69d2ac 100755
--- a/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
+++ b/pps_ws/src/d_fall_pps/param/ClientConfig.yaml
@@ -1,5 +1,5 @@
safeController: "SafeControllerService/RateController"
-customController: "CircleControllerService/CircleController"
+customController: "CustomControllerService/CustomController"
strictSafety: true
angleMargin: 0.6
diff --git a/pps_ws/src/d_fall_pps/param/SafeController.yaml b/pps_ws/src/d_fall_pps/param/SafeController.yaml
index 1997d4da..d660059e 100644
--- a/pps_ws/src/d_fall_pps/param/SafeController.yaml
+++ b/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
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index d9fd6949..a3b8aa6e 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -1,5 +1,5 @@
// 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();
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index e212fe83..b3d2ebbe 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/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;
diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
index f0f7e5b4..6f1000d9 100755
--- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp
+++ b/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);
--
GitLab