From 25a9aa350947af2a0d94d72b5d67b7b6f90af66b Mon Sep 17 00:00:00 2001
From: Sem van der Hoeven <semmer99@gmail.com>
Date: Mon, 15 May 2023 10:51:56 +0200
Subject: [PATCH] add logging of receiving correct trajectory setpoint message

---
 src/px4_connection/src/px4_controller.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/px4_connection/src/px4_controller.cpp b/src/px4_connection/src/px4_controller.cpp
index 917daf92..b01f7803 100644
--- a/src/px4_connection/src/px4_controller.cpp
+++ b/src/px4_connection/src/px4_controller.cpp
@@ -156,12 +156,14 @@ private:
         }
         else
         {
+            RCLCPP_INFO(this->get_logger(), "Got new trajectory setpoint with control mode: %d", request->control_mode);
             if (request->control_mode == CONTROL_MODE_VELOCITY)
             {
                 for (int i = 0; i < 3; i++)
                 {
                     velocity[i] = request->values[i];
                 }
+                RCLCPP_INFO(this->get_logger(), "Got new velocity setpoint: %f %f %f", velocity[0], velocity[1], velocity[2]);
             }
             else if (request->control_mode == CONTROL_MODE_POSITION)
             {
@@ -169,6 +171,7 @@ private:
                 {
                     position[i] = request->values[i];
                 }
+                RCLCPP_INFO(this->get_logger(), "Got new position setpoint: %f %f %f", position[0], position[1], position[2]);
             }
 
             last_angle = request->yaw;