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.

pps_ws/src/d_fall_pps/param/SafeController.yaml

@@ -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.0, 0.0, 0.1, 0.0]
 

pps_ws/src/d_fall_pps/src/PPSClient.cpp

@@ -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);
-		
 	}
 }
 

pps_ws/src/d_fall_pps/src/SafeControllerService.cpp

@@ -17,6 +17,11 @@
 #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"
@@ -48,6 +53,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 +115,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,6 +166,8 @@ 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;
 	//<<<<<<
@@ -167,6 +177,8 @@ 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;
@@ -174,6 +186,11 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
 
     float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
     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
     convertIntoBodyFrame(est, state, yaw_measured);
@@ -193,7 +210,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 +229,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 +256,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;
 }

pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp

@@ -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;