From be8e7d79efc3021f1170d8441ee0d21e6b46ac96 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Wed, 16 May 2018 10:52:58 +0200 Subject: [PATCH 01/12] Updates in the TravisCI file Some updates in the TravisCI file --- .travis.yml | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/.travis.yml b/.travis.yml index 11d056c..6c2db37 100644 --- a/.travis.yml +++ b/.travis.yml @@ -19,12 +19,13 @@ # `wstool`, and should be listed in a file named dependencies.rosinstall. # # There are two variables you may want to change: -# - ROS_DISTRO (default is indigo). Note that packages must be available for -# ubuntu 14.04 trusty. +# - ROS_DISTRO (default is kinetic). Note that packages must be available for +# ubuntu 16.04 trusty. # # See the README.md for more information. # # Author: Felix Duvallet +# Author: Giuseppe Silano # NOTE: The build lifecycle on Travis.ci is something like this: # before_install @@ -66,7 +67,7 @@ env: # Install system dependencies, namely a very barebones ROS setup. before_install: - - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list' + - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main" > /etc/apt/sources.list.d/ros-latest.list' - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - - sudo rm /var/lib/dpkg/lock - sudo dpkg --configure -a From b774f392f0f0add8995e3d66a856b5de053abdb6 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Thu, 24 May 2018 10:15:21 +0200 Subject: [PATCH 02/12] Sim time has been changed The sim time has been changed: from system to simulation clock. Now, we are sure that the controller algorithm uses the Gazebo simulation time. --- launch/task1_world.launch | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/task1_world.launch b/launch/task1_world.launch index 79a458e..0b3205e 100644 --- a/launch/task1_world.launch +++ b/launch/task1_world.launch @@ -35,6 +35,7 @@ + From e21610501384bcc4bdd590cd80d763d3270d0a14 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Thu, 24 May 2018 15:42:05 +0200 Subject: [PATCH 03/12] Updates in data storage section In the data storage section, simulation times of position and position errors have been added. Such times are caught by invoking a gazebo service. In this way, for each data there is associated a simulation time stamp. --- .../position_controller.h | 11 +- src/library/position_controller.cpp | 120 +++++++++++------- 2 files changed, 86 insertions(+), 45 deletions(-) diff --git a/include/teamsannio_med_control/position_controller.h b/include/teamsannio_med_control/position_controller.h index 0f68c9d..88717be 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -32,6 +32,8 @@ #include "parameters.h" #include "common.h" +#include + using namespace std; @@ -104,8 +106,13 @@ class PositionControllerParameters { EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: + //Boolean variables to active/unactive the controller and the data storage bool controller_active_; - bool dataStoring_active_; + bool dataStoring_active_; + + //Gazebo Message for attitude and position + gazebo_msgs::GetWorldProperties my_messageAttitude_; + gazebo_msgs::GetWorldProperties my_messagePosition_; //Sting vectors used to stare data std::vector listControlSignals_; @@ -117,6 +124,8 @@ class PositionControllerParameters { std::vector listTrajectoryErrors_; std::vector listAttitudeErrors_; std::vector listDerivativeAttitudeErrors_; + std::vector listTimeAttitudeErrors_; + std::vector listTimePositionErrors_; //Controller gains double beta_x_, beta_y_, beta_z_; diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 361d1d3..947218d 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -40,7 +40,7 @@ #define M_PI 3.14159265358979323846 /* pi */ #define TsP 10e-3 /* Position control sampling time */ #define TsA 5e-3 /* Attitude control sampling time */ -#define storeTime 3 /* Store time*/ +#define storeTime 15 /* Store time*/ using namespace std; @@ -48,7 +48,7 @@ namespace teamsannio_med_control { PositionController::PositionController() : controller_active_(false), - dataStoring_active_(false), + dataStoring_active_(false), e_x_(0), e_y_(0), e_z_(0), @@ -64,10 +64,11 @@ PositionController::PositionController() timer1_ = n1_.createTimer(ros::Duration(TsA), &PositionController::CallbackAttitude, this, false, true); timer2_ = n2_.createTimer(ros::Duration(TsP), &PositionController::CallbackPosition, this, false, true); - //this serves to inactivate the controller if we don't recieve a trajectory - - if(dataStoring_active_){ - timer3_ = n3_.createTimer(ros::Duration(storeTime), &PositionController::CallbackSaveData, this, false, true); + + + //this serves to inactivate the logging if we wan't to save the simulated data + if(dataStoring_active_){ + timer3_ = n3_.createTimer(ros::Duration(storeTime), &PositionController::CallbackSaveData, this, false, true); //Cleaning the string vector contents listControlSignals_.clear(); @@ -79,7 +80,9 @@ PositionController::PositionController() listDroneAttitude_.clear(); listTrajectoryErrors_.clear(); listAttitudeErrors_.clear(); - listDerivativeAttitudeErrors_.clear(); + listDerivativeAttitudeErrors_.clear(); + listTimeAttitudeErrors_.clear(); + listTimePositionErrors_.clear(); } @@ -101,21 +104,25 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ ofstream fileTrajectoryErrors; ofstream fileAttitudeErrors; ofstream fileDerivativeAttitudeErrors; + ofstream fileTimeAttitudeErrors; + ofstream fileTimePositionErrors; ROS_INFO("CallbackSavaData function is working. Time: %f seconds, %f nanoseconds", odometry_.timeStampSec, odometry_.timeStampNsec); - fileControllerGains.open ("/home/giuseppe/Scrivania/controllerGains.csv", std::ios_base::app); - fileVehicleParameters.open ("/home/giuseppe/Scrivania/vehicleParamters.csv", std::ios_base::app); - fileControlSignals.open ("/home/giuseppe/Scrivania/controlSignals.csv", std::ios_base::app); - fileControlMixerTerms.open ("/home/giuseppe/Scrivania/controlMixer.csv", std::ios_base::app); - filePropellersAngularVelocities.open ("/home/giuseppe/Scrivania/propellersAngularVelocities.csv", std::ios_base::app); - fileReferenceAngles.open ("/home/giuseppe/Scrivania/referenceAngles.csv", std::ios_base::app); - fileVelocityErrors.open ("/home/giuseppe/Scrivania/velocityErrors.csv", std::ios_base::app); - fileDroneAttiude.open ("/home/giuseppe/Scrivania/droneAttitude.csv", std::ios_base::app); - fileTrajectoryErrors.open ("/home/giuseppe/Scrivania/trajectoryErrors.csv", std::ios_base::app); - fileAttitudeErrors.open ("/home/giuseppe/Scrivania/attitudeErrors.csv", std::ios_base::app); - fileDerivativeAttitudeErrors.open ("/home/giuseppe/Scrivania/derivativeAttitudeErrors.csv", std::ios_base::app); + fileControllerGains.open ("/home/giuseppe/controllerGains.csv", std::ios_base::app); + fileVehicleParameters.open ("/home/giuseppe/vehicleParamters.csv", std::ios_base::app); + fileControlSignals.open ("/home/giuseppe/controlSignals.csv", std::ios_base::app); + fileControlMixerTerms.open ("/home/giuseppe/controlMixer.csv", std::ios_base::app); + filePropellersAngularVelocities.open ("/home/giuseppe/propellersAngularVelocities.csv", std::ios_base::app); + fileReferenceAngles.open ("/home/giuseppe/referenceAngles.csv", std::ios_base::app); + fileVelocityErrors.open ("/home/giuseppe/velocityErrors.csv", std::ios_base::app); + fileDroneAttiude.open ("/home/giuseppe/droneAttitude.csv", std::ios_base::app); + fileTrajectoryErrors.open ("/home/giuseppe/trajectoryErrors.csv", std::ios_base::app); + fileAttitudeErrors.open ("/home/giuseppe/attitudeErrors.csv", std::ios_base::app); + fileDerivativeAttitudeErrors.open ("/home/giuseppe/derivativeAttitudeErrors.csv", std::ios_base::app); + fileTimeAttitudeErrors.open ("/home/giuseppe/timeAttitudeErrors.csv", std::ios_base::app); + fileTimePositionErrors.open ("/home/giuseppe/timePositionErrors.csv", std::ios_base::app); //Saving vehicle parameters in a file fileControllerGains << beta_x_ << "," << beta_y_ << "," << beta_z_ << "," << alpha_x_ << "," << alpha_y_ << "," << alpha_z_ << "," << beta_phi_ << "," << beta_theta_ << "," << beta_psi_ << "," << alpha_phi_ << "," << alpha_theta_ << "," << alpha_psi_ << "," << mu_x_ << "," << mu_y_ << "," << mu_z_ << "," << mu_phi_ << "," << mu_theta_ << "," << mu_psi_ << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; @@ -143,7 +150,7 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileReferenceAngles << listReferenceAngles_.at( n ); } - //Saving the velcoty errors in a file + //Saving the velocity errors in a file for (unsigned n=0; n < listVelocityErrors_.size(); ++n) { fileVelocityErrors << listVelocityErrors_.at( n ); } @@ -168,6 +175,14 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileDerivativeAttitudeErrors << listDerivativeAttitudeErrors_.at( n ); } + //Saving the position and attitude errors times + for (unsigned n=0; n < listTimeAttitudeErrors_.size(); ++n) { + fileTimeAttitudeErrors << listTimeAttitudeErrors_.at( n ); + } + + for (unsigned n=0; n < listTimePositionErrors_.size(); ++n) { + fileTimePositionErrors << listTimePositionErrors_.at( n ); + } //Closing all opened files fileControllerGains.close (); @@ -181,6 +196,8 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileTrajectoryErrors.close(); fileAttitudeErrors.close(); fileDerivativeAttitudeErrors.close(); + fileTimeAttitudeErrors.close(); + fileTimePositionErrors.close(); } @@ -258,42 +275,51 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit AttitudeController(&u_phi, &u_theta, &u_psi); PosController(&u_x, &u_y, &u_T, &u_Terr); - if(dataStoring_active_){ - //Saving control signals in a file - std::stringstream tempControlSignals; - tempControlSignals << u_T << "," << u_phi << "," << u_theta << "," << u_psi << "," << u_x << "," << u_y << "," << u_Terr << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; + if(dataStoring_active_){ + //Saving control signals in a file + std::stringstream tempControlSignals; + tempControlSignals << u_T << "," << u_phi << "," << u_theta << "," << u_psi << "," << u_x << "," << u_y << "," << u_Terr << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; + + listControlSignals_.push_back(tempControlSignals.str()); + + //Saving drone attitude in a file + std::stringstream tempDroneAttitude; + tempDroneAttitude << state_.attitude.roll << "," << state_.attitude.pitch << "," << state_.attitude.yaw << "," <("/gazebo/get_world_properties"); + clientAttitude.call(my_messageAttitude_); } void PositionController::CallbackPosition(const ros::TimerEvent& event){ PositionErrors(&e_x_, &e_y_, &e_z_); VelocityErrors(&dot_e_x_, &dot_e_y_, &dot_e_z_); + ros::NodeHandle clientHandlePosition; + ros::ServiceClient clientPosition = clientHandlePosition.serviceClient("/gazebo/get_world_properties"); + clientPosition.call(my_messagePosition_); } From f3c04652a4de40baced5a9f6e43ffbb6f573f55d Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Thu, 24 May 2018 15:42:05 +0200 Subject: [PATCH 04/12] Updates into the data storage section In the data storage section, simulation times of position and position errors have been added. Such times are caught by invoking a gazebo service. In this way, for each data there is associated a simulation time stamp. --- .../position_controller.h | 11 +- src/library/position_controller.cpp | 120 +++++++++++------- 2 files changed, 86 insertions(+), 45 deletions(-) diff --git a/include/teamsannio_med_control/position_controller.h b/include/teamsannio_med_control/position_controller.h index 0f68c9d..88717be 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -32,6 +32,8 @@ #include "parameters.h" #include "common.h" +#include + using namespace std; @@ -104,8 +106,13 @@ class PositionControllerParameters { EIGEN_MAKE_ALIGNED_OPERATOR_NEW private: + //Boolean variables to active/unactive the controller and the data storage bool controller_active_; - bool dataStoring_active_; + bool dataStoring_active_; + + //Gazebo Message for attitude and position + gazebo_msgs::GetWorldProperties my_messageAttitude_; + gazebo_msgs::GetWorldProperties my_messagePosition_; //Sting vectors used to stare data std::vector listControlSignals_; @@ -117,6 +124,8 @@ class PositionControllerParameters { std::vector listTrajectoryErrors_; std::vector listAttitudeErrors_; std::vector listDerivativeAttitudeErrors_; + std::vector listTimeAttitudeErrors_; + std::vector listTimePositionErrors_; //Controller gains double beta_x_, beta_y_, beta_z_; diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 361d1d3..947218d 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -40,7 +40,7 @@ #define M_PI 3.14159265358979323846 /* pi */ #define TsP 10e-3 /* Position control sampling time */ #define TsA 5e-3 /* Attitude control sampling time */ -#define storeTime 3 /* Store time*/ +#define storeTime 15 /* Store time*/ using namespace std; @@ -48,7 +48,7 @@ namespace teamsannio_med_control { PositionController::PositionController() : controller_active_(false), - dataStoring_active_(false), + dataStoring_active_(false), e_x_(0), e_y_(0), e_z_(0), @@ -64,10 +64,11 @@ PositionController::PositionController() timer1_ = n1_.createTimer(ros::Duration(TsA), &PositionController::CallbackAttitude, this, false, true); timer2_ = n2_.createTimer(ros::Duration(TsP), &PositionController::CallbackPosition, this, false, true); - //this serves to inactivate the controller if we don't recieve a trajectory - - if(dataStoring_active_){ - timer3_ = n3_.createTimer(ros::Duration(storeTime), &PositionController::CallbackSaveData, this, false, true); + + + //this serves to inactivate the logging if we wan't to save the simulated data + if(dataStoring_active_){ + timer3_ = n3_.createTimer(ros::Duration(storeTime), &PositionController::CallbackSaveData, this, false, true); //Cleaning the string vector contents listControlSignals_.clear(); @@ -79,7 +80,9 @@ PositionController::PositionController() listDroneAttitude_.clear(); listTrajectoryErrors_.clear(); listAttitudeErrors_.clear(); - listDerivativeAttitudeErrors_.clear(); + listDerivativeAttitudeErrors_.clear(); + listTimeAttitudeErrors_.clear(); + listTimePositionErrors_.clear(); } @@ -101,21 +104,25 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ ofstream fileTrajectoryErrors; ofstream fileAttitudeErrors; ofstream fileDerivativeAttitudeErrors; + ofstream fileTimeAttitudeErrors; + ofstream fileTimePositionErrors; ROS_INFO("CallbackSavaData function is working. Time: %f seconds, %f nanoseconds", odometry_.timeStampSec, odometry_.timeStampNsec); - fileControllerGains.open ("/home/giuseppe/Scrivania/controllerGains.csv", std::ios_base::app); - fileVehicleParameters.open ("/home/giuseppe/Scrivania/vehicleParamters.csv", std::ios_base::app); - fileControlSignals.open ("/home/giuseppe/Scrivania/controlSignals.csv", std::ios_base::app); - fileControlMixerTerms.open ("/home/giuseppe/Scrivania/controlMixer.csv", std::ios_base::app); - filePropellersAngularVelocities.open ("/home/giuseppe/Scrivania/propellersAngularVelocities.csv", std::ios_base::app); - fileReferenceAngles.open ("/home/giuseppe/Scrivania/referenceAngles.csv", std::ios_base::app); - fileVelocityErrors.open ("/home/giuseppe/Scrivania/velocityErrors.csv", std::ios_base::app); - fileDroneAttiude.open ("/home/giuseppe/Scrivania/droneAttitude.csv", std::ios_base::app); - fileTrajectoryErrors.open ("/home/giuseppe/Scrivania/trajectoryErrors.csv", std::ios_base::app); - fileAttitudeErrors.open ("/home/giuseppe/Scrivania/attitudeErrors.csv", std::ios_base::app); - fileDerivativeAttitudeErrors.open ("/home/giuseppe/Scrivania/derivativeAttitudeErrors.csv", std::ios_base::app); + fileControllerGains.open ("/home/giuseppe/controllerGains.csv", std::ios_base::app); + fileVehicleParameters.open ("/home/giuseppe/vehicleParamters.csv", std::ios_base::app); + fileControlSignals.open ("/home/giuseppe/controlSignals.csv", std::ios_base::app); + fileControlMixerTerms.open ("/home/giuseppe/controlMixer.csv", std::ios_base::app); + filePropellersAngularVelocities.open ("/home/giuseppe/propellersAngularVelocities.csv", std::ios_base::app); + fileReferenceAngles.open ("/home/giuseppe/referenceAngles.csv", std::ios_base::app); + fileVelocityErrors.open ("/home/giuseppe/velocityErrors.csv", std::ios_base::app); + fileDroneAttiude.open ("/home/giuseppe/droneAttitude.csv", std::ios_base::app); + fileTrajectoryErrors.open ("/home/giuseppe/trajectoryErrors.csv", std::ios_base::app); + fileAttitudeErrors.open ("/home/giuseppe/attitudeErrors.csv", std::ios_base::app); + fileDerivativeAttitudeErrors.open ("/home/giuseppe/derivativeAttitudeErrors.csv", std::ios_base::app); + fileTimeAttitudeErrors.open ("/home/giuseppe/timeAttitudeErrors.csv", std::ios_base::app); + fileTimePositionErrors.open ("/home/giuseppe/timePositionErrors.csv", std::ios_base::app); //Saving vehicle parameters in a file fileControllerGains << beta_x_ << "," << beta_y_ << "," << beta_z_ << "," << alpha_x_ << "," << alpha_y_ << "," << alpha_z_ << "," << beta_phi_ << "," << beta_theta_ << "," << beta_psi_ << "," << alpha_phi_ << "," << alpha_theta_ << "," << alpha_psi_ << "," << mu_x_ << "," << mu_y_ << "," << mu_z_ << "," << mu_phi_ << "," << mu_theta_ << "," << mu_psi_ << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; @@ -143,7 +150,7 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileReferenceAngles << listReferenceAngles_.at( n ); } - //Saving the velcoty errors in a file + //Saving the velocity errors in a file for (unsigned n=0; n < listVelocityErrors_.size(); ++n) { fileVelocityErrors << listVelocityErrors_.at( n ); } @@ -168,6 +175,14 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileDerivativeAttitudeErrors << listDerivativeAttitudeErrors_.at( n ); } + //Saving the position and attitude errors times + for (unsigned n=0; n < listTimeAttitudeErrors_.size(); ++n) { + fileTimeAttitudeErrors << listTimeAttitudeErrors_.at( n ); + } + + for (unsigned n=0; n < listTimePositionErrors_.size(); ++n) { + fileTimePositionErrors << listTimePositionErrors_.at( n ); + } //Closing all opened files fileControllerGains.close (); @@ -181,6 +196,8 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileTrajectoryErrors.close(); fileAttitudeErrors.close(); fileDerivativeAttitudeErrors.close(); + fileTimeAttitudeErrors.close(); + fileTimePositionErrors.close(); } @@ -258,42 +275,51 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit AttitudeController(&u_phi, &u_theta, &u_psi); PosController(&u_x, &u_y, &u_T, &u_Terr); - if(dataStoring_active_){ - //Saving control signals in a file - std::stringstream tempControlSignals; - tempControlSignals << u_T << "," << u_phi << "," << u_theta << "," << u_psi << "," << u_x << "," << u_y << "," << u_Terr << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; + if(dataStoring_active_){ + //Saving control signals in a file + std::stringstream tempControlSignals; + tempControlSignals << u_T << "," << u_phi << "," << u_theta << "," << u_psi << "," << u_x << "," << u_y << "," << u_Terr << "," << odometry_.timeStampSec << "," << odometry_.timeStampNsec << "\n"; + + listControlSignals_.push_back(tempControlSignals.str()); + + //Saving drone attitude in a file + std::stringstream tempDroneAttitude; + tempDroneAttitude << state_.attitude.roll << "," << state_.attitude.pitch << "," << state_.attitude.yaw << "," <("/gazebo/get_world_properties"); + clientAttitude.call(my_messageAttitude_); } void PositionController::CallbackPosition(const ros::TimerEvent& event){ PositionErrors(&e_x_, &e_y_, &e_z_); VelocityErrors(&dot_e_x_, &dot_e_y_, &dot_e_z_); + ros::NodeHandle clientHandlePosition; + ros::ServiceClient clientPosition = clientHandlePosition.serviceClient("/gazebo/get_world_properties"); + clientPosition.call(my_messagePosition_); } From 81cc321f0d10bb2d782938e91591cc374ea31ae1 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Thu, 24 May 2018 16:31:26 +0200 Subject: [PATCH 05/12] Deleted function prototype not longer necessary Deleted functions prototypes not longer necessary --- src/nodes/position_controller_node.h | 8 -------- 1 file changed, 8 deletions(-) diff --git a/src/nodes/position_controller_node.h b/src/nodes/position_controller_node.h index 3996443..753d415 100644 --- a/src/nodes/position_controller_node.h +++ b/src/nodes/position_controller_node.h @@ -60,16 +60,8 @@ namespace teamsannio_med_control { //publisher ros::Publisher motor_velocity_reference_pub_; - mav_msgs::EigenTrajectoryPointDeque commands_; - std::deque command_waiting_times_; - ros::Timer command_timer_; - - void TimedCommandCallback(const ros::TimerEvent& e); - void MultiDofJointTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); - void CommandPoseCallback(const geometry_msgs::PoseStampedConstPtr& pose_msg); - void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); From 833dba9a4ded1585b5122018f007be72ed4da76d Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Thu, 24 May 2018 16:47:42 +0200 Subject: [PATCH 06/12] Bug fix --- src/nodes/position_controller_node.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/nodes/position_controller_node.h b/src/nodes/position_controller_node.h index 753d415..f2c4a82 100644 --- a/src/nodes/position_controller_node.h +++ b/src/nodes/position_controller_node.h @@ -60,6 +60,10 @@ namespace teamsannio_med_control { //publisher ros::Publisher motor_velocity_reference_pub_; + mav_msgs::EigenTrajectoryPointDeque commands_; + std::deque command_waiting_times_; + ros::Timer command_timer_; + void MultiDofJointTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); From 83b070e0d95e19be13752b1422cf470b0ae5273a Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Fri, 25 May 2018 11:44:54 +0200 Subject: [PATCH 07/12] Fixed typos in the data storing section Fixed typos in the data storing section into the position controller file. Now, the algorithm is able to take into account the wall clock and the simulated time when "use_sim_time" is unactive --- CHANGELOG.rst | 7 +++ .../position_controller.h | 8 ++- launch/task1_world.launch | 2 +- src/library/position_controller.cpp | 53 +++++++++++-------- 4 files changed, 47 insertions(+), 23 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 660ebd0..4b0e7f5 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package teamsannio_med_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.3 (2018-05-25) +----------- +* added the data storage section. Such section inserted in the position controller node allows to storage, in csv files, the aircraft and controller state. +* bug fixing +* the sim time has been added in the launch file. Now the position controller node uses the simulation and not wall clock time. +* Contributors: Giuseppe Silano, Luigi Iannelli + 0.1.2 (2018-05-15) ----------- * added the basic.world file inside the repository diff --git a/include/teamsannio_med_control/position_controller.h b/include/teamsannio_med_control/position_controller.h index 88717be..d95c9d0 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -111,8 +111,14 @@ class PositionControllerParameters { bool dataStoring_active_; //Gazebo Message for attitude and position - gazebo_msgs::GetWorldProperties my_messageAttitude_; gazebo_msgs::GetWorldProperties my_messagePosition_; + ros::NodeHandle clientHandlePosition_; + ros::ServiceClient clientPosition_; + + ros::NodeHandle clientHandleAttitude_; + ros::ServiceClient clientAttitude_; + gazebo_msgs::GetWorldProperties my_messageAttitude_; + //Sting vectors used to stare data std::vector listControlSignals_; diff --git a/launch/task1_world.launch b/launch/task1_world.launch index 0b3205e..601adce 100644 --- a/launch/task1_world.launch +++ b/launch/task1_world.launch @@ -35,7 +35,7 @@ - + diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 947218d..47ace8a 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -72,7 +72,7 @@ PositionController::PositionController() //Cleaning the string vector contents listControlSignals_.clear(); - listControlSignals_.clear(); + listControlSignals_.clear(); listControlMixerTerms_.clear(); listPropellersAngularVelocities_.clear(); listReferenceAngles_.clear(); @@ -83,6 +83,11 @@ PositionController::PositionController() listDerivativeAttitudeErrors_.clear(); listTimeAttitudeErrors_.clear(); listTimePositionErrors_.clear(); + + //the client needed to get information about the Gazebo simulation environment both the attitude and position errors + clientAttitude_ = clientHandleAttitude_.serviceClient("/gazebo/get_world_properties"); + clientPosition_ = clientHandlePosition_.serviceClient("/gazebo/get_world_properties"); + } @@ -175,7 +180,7 @@ void PositionController::CallbackSaveData(const ros::TimerEvent& event){ fileDerivativeAttitudeErrors << listDerivativeAttitudeErrors_.at( n ); } - //Saving the position and attitude errors times + //Saving the position and attitude errors along the time for (unsigned n=0; n < listTimeAttitudeErrors_.size(); ++n) { fileTimeAttitudeErrors << listTimeAttitudeErrors_.at( n ); } @@ -288,34 +293,25 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit listDroneAttitude_.push_back(tempDroneAttitude.str()); - //Saving the attitude and position computing errors time - std::stringstream tempTimeAttitudeErrors; - std::stringstream tempTimePositionErrors; - tempTimeAttitudeErrors << my_messageAttitude_.response.sim_time << "\n"; - tempTimePositionErrors << my_messagePosition_.response.sim_time << "\n"; - - listTimeAttitudeErrors_.push_back(tempTimePositionErrors.str()); - listTimeAttitudeErrors_.push_back(tempTimeAttitudeErrors.str()); - //Saving velocity errors in a file std::stringstream tempVelocityErrors; - tempVelocityErrors << dot_e_x_ << "," << dot_e_y_ << "," << dot_e_z_ << "," <("/gazebo/get_world_properties"); - clientAttitude.call(my_messageAttitude_); + + //Saving the time instant when the attitude errors are computed + if(dataStoring_active_){ + + clientAttitude_.call(my_messageAttitude_); + + std::stringstream tempTimeAttitudeErrors; + tempTimeAttitudeErrors << my_messageAttitude_.response.sim_time << "\n"; + listTimeAttitudeErrors_.push_back(tempTimeAttitudeErrors.str()); + + } } void PositionController::CallbackPosition(const ros::TimerEvent& event){ PositionErrors(&e_x_, &e_y_, &e_z_); VelocityErrors(&dot_e_x_, &dot_e_y_, &dot_e_z_); - ros::NodeHandle clientHandlePosition; - ros::ServiceClient clientPosition = clientHandlePosition.serviceClient("/gazebo/get_world_properties"); - clientPosition.call(my_messagePosition_); + + //Saving the time instant when the position errors are computed + if(dataStoring_active_){ + clientPosition_.call(my_messagePosition_); + + std::stringstream tempTimePositionErrors; + tempTimePositionErrors << my_messagePosition_.response.sim_time << "\n"; + listTimePositionErrors_.push_back(tempTimePositionErrors.str()); + + } } From 665e4ce78411d3c2caefe6a007aec7ff0616cd97 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Fri, 25 May 2018 13:23:55 +0200 Subject: [PATCH 08/12] Updates in the storing section The wall clock time has been inserted in the data storing section. Now, it is possibile to kwnon for each messagge the properly wall clock time. --- CHANGELOG.rst | 2 +- .../position_controller.h | 3 ++ src/library/position_controller.cpp | 28 +++++++++++-------- 3 files changed, 21 insertions(+), 12 deletions(-) diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 4b0e7f5..0d9c100 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -4,7 +4,7 @@ Changelog for package teamsannio_med_control 0.1.3 (2018-05-25) ----------- -* added the data storage section. Such section inserted in the position controller node allows to storage, in csv files, the aircraft and controller state. +* added the data storage section. Such section has been inserted in the position controller node and allows to storage, in defined csv files, the aircraft and controller state. * bug fixing * the sim time has been added in the launch file. Now the position controller node uses the simulation and not wall clock time. * Contributors: Giuseppe Silano, Luigi Iannelli diff --git a/include/teamsannio_med_control/position_controller.h b/include/teamsannio_med_control/position_controller.h index d95c9d0..d14a40e 100644 --- a/include/teamsannio_med_control/position_controller.h +++ b/include/teamsannio_med_control/position_controller.h @@ -110,6 +110,9 @@ class PositionControllerParameters { bool controller_active_; bool dataStoring_active_; + //Wall clock time offset variable + double wallSecsOffset_; + //Gazebo Message for attitude and position gazebo_msgs::GetWorldProperties my_messagePosition_; ros::NodeHandle clientHandlePosition_; diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 47ace8a..642c3cf 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -23,6 +23,7 @@ #include "teamsannio_med_control/stabilizer_types.h" #include +#include #include #include #include @@ -87,6 +88,9 @@ PositionController::PositionController() //the client needed to get information about the Gazebo simulation environment both the attitude and position errors clientAttitude_ = clientHandleAttitude_.serviceClient("/gazebo/get_world_properties"); clientPosition_ = clientHandlePosition_.serviceClient("/gazebo/get_world_properties"); + + ros::WallTime beginWallOffset = ros::WallTime::now(); + wallSecsOffset_ = beginWallOffset.toSec(); } @@ -293,25 +297,28 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit listDroneAttitude_.push_back(tempDroneAttitude.str()); + ros::WallTime beginWall = ros::WallTime::now(); + double wallSecs = beginWall.toSec() - wallSecsOffset_; + //Saving velocity errors in a file std::stringstream tempVelocityErrors; - tempVelocityErrors << dot_e_x_ << "," << dot_e_y_ << "," << dot_e_z_ << "," < Date: Fri, 25 May 2018 14:47:32 +0200 Subject: [PATCH 09/12] Data storing section updated Now the attitude and the position errors are updated only when the event occurs. In other words, every Tsa and Tsp time, respectively. --- src/library/position_controller.cpp | 70 ++++++++++++++++------------- 1 file changed, 40 insertions(+), 30 deletions(-) diff --git a/src/library/position_controller.cpp b/src/library/position_controller.cpp index 642c3cf..da17056 100644 --- a/src/library/position_controller.cpp +++ b/src/library/position_controller.cpp @@ -297,32 +297,6 @@ void PositionController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocit listDroneAttitude_.push_back(tempDroneAttitude.str()); - ros::WallTime beginWall = ros::WallTime::now(); - double wallSecs = beginWall.toSec() - wallSecsOffset_; - - //Saving velocity errors in a file - std::stringstream tempVelocityErrors; - tempVelocityErrors << dot_e_x_ << "," << dot_e_y_ << "," << dot_e_z_ << "," < Date: Mon, 28 May 2018 11:43:06 +0200 Subject: [PATCH 10/12] Use sim time setted as parameter Use sim time setted as parameter in the launch file. --- launch/task1_world.launch | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/launch/task1_world.launch b/launch/task1_world.launch index 601adce..b3e18bd 100644 --- a/launch/task1_world.launch +++ b/launch/task1_world.launch @@ -3,6 +3,7 @@ + @@ -35,7 +36,7 @@ - + From 105247555e316151c153a397ab51940750c731eb Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Wed, 30 May 2018 15:40:42 +0200 Subject: [PATCH 11/12] Added plot in the launch file Added plot in the launch file. Now, the drone position (x, y and z) is depicted via a graphic when the simulation runs. --- launch/task1_world.launch | 3 +++ 1 file changed, 3 insertions(+) diff --git a/launch/task1_world.launch b/launch/task1_world.launch index b3e18bd..b9da83c 100644 --- a/launch/task1_world.launch +++ b/launch/task1_world.launch @@ -47,4 +47,7 @@ + + + From 25a4710796cb0489259296055d173c414fc21f25 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Mon, 4 Jun 2018 11:17:38 +0200 Subject: [PATCH 12/12] Updates in the TravisCI file The part related to notification via mail has been added into the file. --- .travis.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.travis.yml b/.travis.yml index 6c2db37..c2f5da0 100644 --- a/.travis.yml +++ b/.travis.yml @@ -99,3 +99,7 @@ install: - catkin build - echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc - source ~/.bashrc + +notifications: + email: + - giuseppe.silano@unisannio.it