To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit b4a2a299 authored by Tobias's avatar Tobias
Browse files

try the rosbag debuggin

parent e28290cd
......@@ -50,6 +50,7 @@
#include "ros/ros.h"
#include <ros/package.h>
#include <Eigen/Dense>
#include <rosbag/bag.h>
// for readMatrix
#include <iostream>
......@@ -69,6 +70,7 @@
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
......@@ -336,4 +338,11 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
void display_control_agent1( Controller::Request &request, Controller::Response &response) ;
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw );
void desired_eulerrates_to_bodyrates( Controller::Request &request , float roll_rate , float pitch_rate, float (&bodyrates)[2]);
\ No newline at end of file
void desired_eulerrates_to_bodyrates( Controller::Request &request , float roll_rate , float pitch_rate, float (&bodyrates)[2]);
// Rosbag for debugging
rosbag::Bag bag_estimator;
rosbag::Bag bag_measurement;
rosbag::Bag bag_input;
rosbag::Bag bag;
\ No newline at end of file
......@@ -556,6 +556,32 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
outYaw = u(11,0);
}
// DEBUGGING
// write estimates to bag
for(int i=0 ; i<33 ; i++)
{
std_msgs::Float32 est_i;
est_i.data = estimator_state_prev(i) ;
std::string topic = "Estimator_" + i;
bag.write(topic,ros::Time::now(),est_i);
}
/* // write input to bag
for(int i=0 ; i<12 ; i++)
{
std_msgs::float32 u_i;
u_i.data = u(i) ;
bag_input.write(u_i);
if(i<11)
{
bag_input.write(",");
}
}
bag_input.write("\n");*/
if (outer_loop_counter % 2 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] --------- ESTIMATES AGENT 1 --------------");
......@@ -605,6 +631,7 @@ void centralized_control_command( Controller::Request &request, Controller::Resp
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired pitch-rate computed = " << outPitch );
ROS_INFO_STREAM("[STUDENT CONTROLLER] desired yaw-rate computed = " << outYaw );
/* float est_error_roll = estimator_state_prev(0)-request.otherCrazyflies[0].roll;
float est_error_pitch = estimator_state_prev(1)-request.otherCrazyflies[0].pitch;
float est_error_yaw = estimator_state_prev(2)-request.otherCrazyflies[0].yaw;
......@@ -941,9 +968,21 @@ void get_y_from_CFData_withLoad( Controller::Request &request )
y_eq(23,0) = load_setpoint[3]; // equilibrium yaw load
// compute y w.r.t. equilibrium position, passed to controller
y_to_controller = y_meas - y_eq;
/*
// Debugging
// write y_tot_controller to bag
for(int i=0 ; i<24 ; i++)
{
std_msgs::float32 y_i;
y_i.data = y_to_controller(i) ;
bag_measurement.write( y_i );
if(i<23)
{
bag_measurement.write(",");
}
bag_measurement.write("\n");*/
if (outer_loop_counter % 2 == 0)
{
ROS_INFO_STREAM("--------------------------" );
......@@ -1833,10 +1872,30 @@ int main(int argc, char* argv[]) {
// Print out some information to the user.
ROS_INFO("[STUDENT CONTROLLER] Service ready :-)");
// Open a ROS "bag" to store data for post-analysis
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO_STREAM(package_path);
std::string record_file = package_path + "debugging.bag";
bag.open(record_file, rosbag::bagmode::Write);
/* std::string record_file = package_path + "estimator.bag";
bag_estimator.open(record_file, rosbag::bagmode::Write);
std::string record_file = package_path + "measurements.bag";
bag_measurement.open(record_file, rosbag::bagmode::Write);
std::string record_file = package_path + "inputs.bag";
bag_input.open(record_file, rosbag::bagmode::Write);*/
// Enter an endless while loop to keep the node alive.
ros::spin();
// Close the ROS "bag" that was opened to store data for post-analysis
/* bag_estimator.close();
bag_measurement.close();
bag_input.close();*/
bag.close();
// Return zero if the "ross::spin" is cancelled.
return 0;
}
\ No newline at end of file
Markdown is supported
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