Commit 939d2e9b authored by muelmarc's avatar muelmarc
Browse files

only some small additions for data recording. safecontroller is also writing...

only some small additions for data recording. safecontroller is also writing to bagfile allowing to find the controller mistake.
parent 6712db3e
...@@ -15,5 +15,5 @@ filterGain: [1, 1, 1, 22.3384, 22.3384, 22.3384] #K_infinite of feedback ...@@ -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 estimatorMatrix: [-22.3384, 0.9106] #only for velocity calculation
#setpoint in meters (x, y, z, yaw) #setpoint in meters (x, y, z, yaw)
defaultSetpoint: [0.0, 0.0, 0.3, 0.0] defaultSetpoint: [0.0, 0.0, 0.1, 0.0]
...@@ -16,8 +16,8 @@ ...@@ -16,8 +16,8 @@
#include "ros/ros.h" #include "ros/ros.h"
#include <stdlib.h> #include <stdlib.h>
#include <rosbag/bag.h>
#include <std_msgs/String.h> #include <std_msgs/String.h>
#include <rosbag/bag.h>
#include <ros/package.h> #include <ros/package.h>
#include "d_fall_pps/Controller.h" #include "d_fall_pps/Controller.h"
...@@ -28,7 +28,7 @@ ...@@ -28,7 +28,7 @@
#include "d_fall_pps/ControlCommand.h" #include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/CrazyflieContext.h" #include "d_fall_pps/CrazyflieContext.h"
#include "std_msgs/Int32.h" #include "std_msgs/Int32.h"
#include "std_msgs/UInt32.h"
#include "d_fall_pps/ControlCommand.h" #include "d_fall_pps/ControlCommand.h"
...@@ -85,9 +85,12 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) { ...@@ -85,9 +85,12 @@ bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
} }
//is called when new data from Vicon arrives //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) { for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
CrazyflieData data = *it; CrazyflieData data = *it;
ROS_INFO_STREAM(data);
if(data.crazyflieName == crazyflieName) { if(data.crazyflieName == crazyflieName) {
Controller controllerCall; Controller controllerCall;
controllerCall.request.ownCrazyflie = data; controllerCall.request.ownCrazyflie = data;
...@@ -112,20 +115,20 @@ void viconCallback(const ViconData& viconData) { ...@@ -112,20 +115,20 @@ void viconCallback(const ViconData& viconData) {
ROS_ERROR("Failed to call safe controller"); 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 } else { //crazyflie disabled
ControlCommand zeroOutput = ControlCommand(); //everything set to zero ControlCommand zeroOutput = ControlCommand(); //everything set to zero
zeroOutput.onboardControllerType = 2; //set to motor_mode zeroOutput.onboardControllerType = 2; //set to motor_mode
controlCommandPublisher.publish(zeroOutput); 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,6 +17,11 @@ ...@@ -17,6 +17,11 @@
#include <math.h> #include <math.h>
#include <stdlib.h> #include <stdlib.h>
#include "ros/ros.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/CrazyflieData.h"
#include "d_fall_pps/Setpoint.h" #include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h" #include "d_fall_pps/ControlCommand.h"
...@@ -48,6 +53,9 @@ float saturationThrust; ...@@ -48,6 +53,9 @@ float saturationThrust;
CrazyflieData previousLocation; CrazyflieData previousLocation;
rosbag::Bag bag;
void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) { void loadParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length) {
if(!nodeHandle.getParam(name, val)){ if(!nodeHandle.getParam(name, val)){
ROS_ERROR_STREAM("missing parameter '" << name << "'"); ROS_ERROR_STREAM("missing parameter '" << name << "'");
...@@ -107,7 +115,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) { ...@@ -107,7 +115,7 @@ void estimateState(Controller::Request &request, float (&est)[9]) {
est[0] = ahat_x[0] + k_x[0]; est[0] = ahat_x[0] + k_x[0];
est[1] = ahat_x[1] + k_x[1]; 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[3] = ahat_x[3] + k_x[3];
est[4] = ahat_x[4] + k_x[4]; est[4] = ahat_x[4] + k_x[4];
est[5] = ahat_x[5] + k_x[5]; est[5] = ahat_x[5] + k_x[5];
...@@ -158,6 +166,8 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) { ...@@ -158,6 +166,8 @@ void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured) {
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) { bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
CrazyflieData vicon = request.ownCrazyflie; CrazyflieData vicon = request.ownCrazyflie;
//bag.write("ViconData", ros::Time::now(), request.ownCrazyflie);
//trial>>>>>>> //trial>>>>>>>
int yaw_measured = request.ownCrazyflie.yaw; int yaw_measured = request.ownCrazyflie.yaw;
//<<<<<< //<<<<<<
...@@ -167,6 +177,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -167,6 +177,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
request.ownCrazyflie.y -= setpoint[1]; request.ownCrazyflie.y -= setpoint[1];
request.ownCrazyflie.z -= setpoint[2]; request.ownCrazyflie.z -= setpoint[2];
float yaw = request.ownCrazyflie.yaw - setpoint[3]; 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;
...@@ -174,6 +186,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -174,6 +186,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
estimateState(request, est); estimateState(request, est);
std_msgs::Float32 EstXTest;
EstXTest.data = est[0];
bag.write("EstimateX", ros::Time::now(), EstXTest);
float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
convertIntoBodyFrame(est, state, yaw_measured); convertIntoBodyFrame(est, state, yaw_measured);
...@@ -193,7 +210,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -193,7 +210,7 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
//HINWEIS: übersteuern beim outYaw wenn man 180 Grad zum yaw-Setpoint startet //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.... //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.roll = outRoll;
response.controlOutput.pitch = outPitch; response.controlOutput.pitch = outPitch;
...@@ -212,6 +229,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response & ...@@ -212,6 +229,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
response.controlOutput.onboardControllerType = RATE_CONTROLLER; response.controlOutput.onboardControllerType = RATE_CONTROLLER;
previousLocation = request.ownCrazyflie; previousLocation = request.ownCrazyflie;
bag.write("ControlOutput", ros::Time::now(), response.controlOutput);
return true; return true;
} }
...@@ -237,7 +256,18 @@ int main(int argc, char* argv[]) { ...@@ -237,7 +256,18 @@ int main(int argc, char* argv[]) {
ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput); ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
ROS_INFO("SafeControllerService ready"); 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(); ros::spin();
bag.close();
return 0; return 0;
} }
...@@ -20,7 +20,7 @@ ...@@ -20,7 +20,7 @@
#include "d_fall_pps/ViconData.h" #include "d_fall_pps/ViconData.h"
#include "d_fall_pps/UnlabeledMarker.h" #include "d_fall_pps/UnlabeledMarker.h"
#define TESTING_FAKE_DATA //#define TESTING_FAKE_DATA
// notice that unit here are in milimeters // notice that unit here are in milimeters
using namespace ViconDataStreamSDK::CPP; 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