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
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]
......@@ -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,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;
}
......@@ -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