Commit b5aa168f authored by roangel's avatar roangel
Browse files

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
parent 13d6ffd6
......@@ -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);
......
......@@ -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 -->
......
safeController: "SafeControllerService/RateController"
customController: "CircleControllerService/CircleController"
customController: "CustomControllerService/CustomController"
strictSafety: true
angleMargin: 0.6
......@@ -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
// 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();
......
......@@ -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;
......
......@@ -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);
......
Supports Markdown
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