From c19251fa26d733fad2de362a467cf1ef9ab12f7d Mon Sep 17 00:00:00 2001
From: Angel <roangel@student.ethz.ch>
Date: Tue, 17 Oct 2017 16:20:47 +0200
Subject: [PATCH] try this in rqt_plot and see
---
pps_ws/src/d_fall_pps/CMakeLists.txt | 2 +-
pps_ws/src/d_fall_pps/msg/Debug.msg | 16 ++++++++++++++++
.../src/CustomControllerService.cpp | 19 +++++++++++++++++++
3 files changed, 36 insertions(+), 1 deletion(-)
create mode 100644 pps_ws/src/d_fall_pps/msg/Debug.msg
diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index e57ee183..db7be180 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -133,7 +133,7 @@ add_message_files(
CrazyflieEntry.msg
CrazyflieDB.msg
#----------------------------------------------------------------------
- Debugging.msg
+ Debug.msg
)
## Generate services in the 'srv' folder
diff --git a/pps_ws/src/d_fall_pps/msg/Debug.msg b/pps_ws/src/d_fall_pps/msg/Debug.msg
new file mode 100644
index 00000000..77b25cd1
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/Debug.msg
@@ -0,0 +1,16 @@
+float64 vicon_x
+float64 vicon_y
+float64 vicon_z
+float64 vicon_yaw
+float64 vicon_pitch
+float64 vicon_roll
+float64 value_1
+float64 value_2
+float64 value_3
+float64 value_4
+float64 value_5
+float64 value_6
+float64 value_7
+float64 value_8
+float64 value_9
+float64 value_10
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
index 6a9603b0..2d88f411 100644
--- a/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/CustomControllerService.cpp
@@ -12,6 +12,7 @@
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/Debug.h"
#include <std_msgs/Int32.h>
@@ -41,6 +42,8 @@ const float gainMatrixYaw[9] = {0, 0, 0, 0, 0, 0, 0, 0, 2.843099534};
const float gainMatrixThrust[9] = {0, 0, 0.22195826, 0, 0, 0.12362477, 0, 0, 0};
+ros::Publisher debugPublisher;
+
// load parameters from corresponding YAML file
@@ -113,6 +116,7 @@ void convertIntoBodyFrame(float est[9], float (&estBody)[9], int yaw_measured)
-response- is where you have to write your calculated data into.
*/
bool calculateControlOutput(Controller::Request &request, Controller::Response &response) {
+
//writing the data from -request- to command line
//might be useful for debugging
// ROS_INFO_STREAM("x-coordinates: " << request.ownCrazyflie.x);
@@ -123,6 +127,19 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// ROS_INFO_STREAM("yaw: " << request.ownCrazyflie.yaw);
// ROS_INFO_STREAM("Delta t: " << request.ownCrazyflie.acquiringTime);
+
+ // ************ Fill the debugging message with information to be displayed in rqt_plot ****************
+ Debug debugMsg;
+ debugMsg.vicon_x = request.ownCrazyflie.x;
+ debugMsg.vicon_y = request.ownCrazyflie.y;
+ debugMsg.vicon_z = request.ownCrazyflie.z;
+ debugMsg.vicon_roll = request.ownCrazyflie.roll;
+ debugMsg.vicon_pitch = request.ownCrazyflie.pitch;
+ debugMsg.vicon_yaw = request.ownCrazyflie.yaw;
+
+ debugPublisher.publish(debugMsg);
+
+
// ********* do your calculations here *********
//Tip: create functions that you call here to keep you code cleaner
ROS_INFO("custom controller loop");
@@ -232,6 +249,8 @@ int main(int argc, char* argv[]) {
ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());
ros::Subscriber customYAMLloadedSubscriber = namespace_nodeHandle.subscribe("student_GUI/customYAMLloaded", 1, customYAMLloadedCallback);
+ ros::Publisher debugPublisher = nodeHandle.advertise<Debug>("Debug", 1);
+
ROS_INFO("CustomControllerService ready");
ros::spin();
--
GitLab