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 8c4366ca authored by roangel's avatar roangel
Browse files

added logging of info to plot in matlab

parent fa7a4cd2
......@@ -147,6 +147,7 @@ add_message_files(
#----------------------------------------------------------------------
DebugMsg.msg
CustomButton.msg
LogMsg.msg
)
## Generate services in the 'srv' folder
......
float64 x
float64 y
float64 z
float64 x_dot
float64 y_dot
float64 z_dot
float64 roll
float64 pitch
float64 yaw
float64 x_sp
float64 y_sp
float64 z_sp
\ No newline at end of file
......@@ -60,12 +60,15 @@
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
#include "d_fall_pps/DebugMsg.h"
#include "d_fall_pps/LogMsg.h"
#include "d_fall_pps/CustomControllerYAML.h"
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
#include <rosbag/bag.h>
#include "MPC_functions.h"
#include "ADP_matrices.h"
......@@ -586,6 +589,11 @@ void do_MPC(int* publish_rate)
}
}
rosbag::Bag bag;
LogMsg log_temp;
ros::Time t_temp;
const double z_offset = 0.09;
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
......@@ -599,10 +607,30 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// read new x0
t_temp = ros::Time::now();
log_temp.x = stateInertialEstimate[0];
log_temp.y = stateInertialEstimate[1];
log_temp.z = stateInertialEstimate[2];
log_temp.x_dot = stateInertialEstimate[3];
log_temp.y_dot = stateInertialEstimate[4];
log_temp.z_dot = stateInertialEstimate[5];
log_temp.roll = stateInertialEstimate[6];
log_temp.pitch = stateInertialEstimate[7];
log_temp.yaw = stateInertialEstimate[8];
log_temp.x_sp = setpoint[0];
log_temp.y_sp = setpoint[1];
log_temp.z_sp = setpoint[2];
bag.write("log_topic", t_temp, log_temp);
// ROS_INFO("wrote in bag");
MPC_mutex.lock();
x0 << stateInertialEstimate[0] - setpoint[0],
stateInertialEstimate[1] - setpoint[1],
stateInertialEstimate[2] - setpoint[2],
stateInertialEstimate[2] - (setpoint[2] - z_offset),
stateInertialEstimate[3],
stateInertialEstimate[4],
stateInertialEstimate[5];
......@@ -1488,14 +1516,25 @@ int main(int argc, char* argv[]) {
// create a 50Hz thread with what we need:
boost::thread MPC_thread(do_MPC, &rate_MPC);
// Open a ROS "bag" to store data for post-analysis
std::string package_path;
package_path = ros::package::getPath("d_fall_pps") + "/";
ROS_INFO("Here is the place where we will save the file: ====================================");
ROS_INFO_STREAM(package_path);
std::string record_file = package_path + "Logging_data.bag";
bag.open(record_file, rosbag::bagmode::Write);
// Print out some information to the user.
ROS_INFO("[MPC CONTROLLER] Service ready :-)");
// Enter an endless while loop to keep the node alive.
ros::spin();
bag.close();
ROS_INFO("bag closed");
// Return zero if the "ross::spin" is cancelled.
// Return zero if the "ross::spin" is cancelled.
MPC_thread.join();
return 0;
}
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