Commit eaf6187c authored by roangel's avatar roangel
Browse files

Merge branch 'pps_project' of https://gitlab.ethz.ch/D-FaLL/D-FaLL-System into...

Merge branch 'pps_project' of https://gitlab.ethz.ch/D-FaLL/D-FaLL-System into from_vicon_data_to_plotting_cfs
parents e6905e43 6b00cd1d
......@@ -112,6 +112,8 @@ add_message_files(
Setpoint.msg
CrazyflieEntry.msg
CrazyflieDB.msg
#----------------------------------------------------------------------
Debugging.msg
)
## Generate services in the 'srv' folder
......
<launch>
<node pkg="d_fall_pps" name="CrazyRadio" output="screen" type="CrazyRadio.py">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient">
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
</node>
<node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="SafeControllerService">
<rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" />
</node>
<node pkg="d_fall_pps" name="CustomControllerService" output="screen" type="CustomControllerService">
</node>
<!-- <node pkg="rqt_plot" name="commandplotter" type="rqt_plot" args="/PPSClient/ControlCommand/roll /PPSClient/ControlCommand/pitch /PPSClient/ControlCommand/yaw" /> -->
</launch>
<launch>
<node pkg="d_fall_pps" name="CentralManagerService" output="screen" type="CentralManagerService">
</node>
<node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher">
<rosparam command="load" file="$(find d_fall_pps)/param/ViconConfig.yaml" />
</node>
<!-- When we have a GUI, this has to be adapted and included -->
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
<!-- <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> -->
</node>
</launch>
float64 x
float64 y
float64 z
float64 vx
float64 vy
float64 vz
float64 roll
float64 pitch
float64 yaw
......@@ -15,5 +15,5 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback
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]
defaultSetpoint: [-0.7, -0.1, 0.25, 0.0]
......@@ -16,8 +16,8 @@
#include "ros/ros.h"
#include <stdlib.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>
#include "d_fall_pps/Controller.h"
......@@ -28,7 +28,7 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Int32.h"
#include "std_msgs/UInt32.h"
#include "d_fall_pps/ControlCommand.h"
......@@ -85,9 +85,12 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
}
//is called when new data from Vicon arrives
void viconCallback(const ViconData& viconData) {
void viconCallback(const ViconData& viconData) {
for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData data = *it;
ROS_INFO_STREAM(data);
if(data.crazyflieName == crazyflieName) {
Controller controllerCall;
controllerCall.request.ownCrazyflie = data;
......@@ -112,20 +115,20 @@ void viconCallback(const ViconData& viconData) {
ROS_ERROR("Failed to call safe controller");
}
}
}
controlCommandPublisher.publish(controllerCall.response.controlOutput);
controlCommandPublisher.publish(controllerCall.response.controlOutput);
bag.write("ViconData", ros::Time::now(), data);
bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
} else { //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
} else { //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput);
bag.write("ViconData", ros::Time::now(), data);
bag.write("ControlOutput", ros::Time::now(), zeroOutput);
}
}
bag.write("ViconData", ros::Time::now(), data);
//bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
}
}
......
......@@ -17,10 +17,16 @@
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h>
#include "std_msgs/Float32.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/Debugging.h" //---------------------------------------------------------------------------
#define PI 3.1415926535
#define RATE_CONTROLLER 0
......@@ -48,6 +54,9 @@ float saturationThrust;
CrazyflieData previousLocation;
rosbag::Bag bag;
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 << "'");
......@@ -107,7 +116,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
est[0] = ahat_x[0] + k_x[0];
est[1] = ahat_x[1] + k_x[1];
est[2] = ahat_x[2] + k_x[2];
est[2] = ahat_x[2] + k_x[2];
est[3] = ahat_x[3] + k_x[3];
est[4] = ahat_x[4] + k_x[4];
est[5] = ahat_x[5] + k_x[5];
......@@ -158,8 +167,10 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
CrazyflieData vicon = request.ownCrazyflie;
//bag.write("ViconData", ros::Time::now(), request.ownCrazyflie);
//trial>>>>>>>
int yaw_measured = request.ownCrazyflie.yaw;
float yaw_measured = request.ownCrazyflie.yaw;
//<<<<<<
//move coordinate system to make setpoint origin
......@@ -167,17 +178,51 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
request.ownCrazyflie.y -= setpoint[1];
request.ownCrazyflie.z -= setpoint[2];
float yaw = request.ownCrazyflie.yaw - setpoint[3];
//bag.write("Offset", ros::Time::now(), request.ownCrazyflie);
while(yaw > PI) yaw -= 2 * PI;
while(yaw < -PI) yaw += 2 * PI;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
request.ownCrazyflie.yaw = yaw;
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est);
//CONTROLLER DEBUGGING--------------------------------------------------------------------------------------------------
Debugging estTests;
estTests.x = est[0];
estTests.y = est[1];
estTests.z = est[2];
estTests.vx = est[3];
estTests.vy = est[4];
estTests.vz = est[5];
estTests.roll = est[6];
estTests.pitch = est[7];
estTests.yaw = est[8];
bag.write("Debugging est", ros::Time::now(), estTests);
//CONTROLLER DEBUGGING END----------------------------------------------------------------------------------------------
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
convertIntoBodyFrame(est, state, yaw_measured);
//convertIntoBodyFrame(est, state, yaw);
//CONTROLLER DEBUGGING--------------------------------------------------------------------------------------------------
estTests.x = state[0];
estTests.y = state[1];
estTests.z = state[2];
estTests.vx = state[3];
estTests.vy = state[4];
estTests.vz = state[5];
estTests.roll = state[6];
estTests.pitch = state[7];
estTests.yaw = state[8];
bag.write("Debugging state", ros::Time::now(), estTests);
std_msgs::Float32 f32;
f32.data = yaw_measured;
bag.write("yaw measured", ros::Time::now(), f32);
//CONTROLLER DEBUGGING END----------------------------------------------------------------------------------------------
//calculate feedback
float outRoll = 0;
......@@ -193,7 +238,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//HINWEIS: übersteuern beim outYaw wenn man 180 Grad zum yaw-Setpoint startet
//nach Multiplikation mit 0.5 gibt es den Effekt nicht mehr -> mit Paul besprechen....
//outYaw *= 0.5;
outYaw *= 0.5;
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
......@@ -212,6 +257,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.onboardControllerType = RATE_CONTROLLER;
previousLocation = request.ownCrazyflie;
bag.write("ControlOutput", ros::Time::now(), response.controlOutput);
return true;
}
......@@ -237,7 +284,18 @@ int main(int argc, char* argv[]) {
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready");
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path);
std::string record_file = package_path + "recordSafeController.bag";
bag.open(record_file, rosbag::bagmode::Write);
ros::spin();
bag.close();
return 0;
}
......@@ -20,7 +20,7 @@
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h"
#define TESTING_FAKE_DATA
//#define TESTING_FAKE_DATA
// notice that unit here are in milimeters
using namespace ViconDataStreamSDK::CPP;
......
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