sem/5g_drone_ROS2
1
0
Fork 0
Code Issues 1 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

add logging of receiving correct trajectory setpoint message

Browse Source
This commit is contained in:
Sem van der Hoeven
2023-05-15 10:51:56 +02:00
parent 8cb032b8d7
commit 25a9aa3509
1 changed files with 3 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/px4_connection/src/px4_controller.cpp
View File

@@ -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;