Commit c19251fa authored by roangel's avatar roangel
Browse files

try this in rqt_plot and see

parent be535040
......@@ -133,7 +133,7 @@ add_message_files(
CrazyflieEntry.msg
CrazyflieDB.msg
#----------------------------------------------------------------------
Debugging.msg
Debug.msg
)
## Generate services in the 'srv' folder
......
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
......@@ -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();
......
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