From fc517810fa8d686f0e0e70867751fa090974cc6d Mon Sep 17 00:00:00 2001 From: Ar-Ray-code Date: Tue, 12 Mar 2024 23:51:09 +0900 Subject: [PATCH] update tf2pos server --- auto_driver_bringup/launch/auto2024.launch.py | 6 +++--- auto_driver_interface/src/tf_to_position_server.cpp | 7 ++++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/auto_driver_bringup/launch/auto2024.launch.py b/auto_driver_bringup/launch/auto2024.launch.py index 544e05a..048f775 100644 --- a/auto_driver_bringup/launch/auto2024.launch.py +++ b/auto_driver_bringup/launch/auto2024.launch.py @@ -31,9 +31,9 @@ def generate_launch_description(): name='pitch_motor', namespace='can_node/gm6020_0', parameters=[ - {'p': 100}, - {'i': 3}, - {'d': 10}, + {'p': 80}, + {'i': 1}, + {'d': 5}, {'max_spd': 3000}, {'init_deg': 120}, {'min_limit': 20}, diff --git a/auto_driver_interface/src/tf_to_position_server.cpp b/auto_driver_interface/src/tf_to_position_server.cpp index 5d39763..7060396 100644 --- a/auto_driver_interface/src/tf_to_position_server.cpp +++ b/auto_driver_interface/src/tf_to_position_server.cpp @@ -103,17 +103,18 @@ namespace auto_driver_interface tf2::Quaternion q(current_rotation.x, current_rotation.y, current_rotation.z, current_rotation.w); tf2::Matrix3x3 m(q); m.getRPY(current_roll, pitch_gap, yaw_gap); + auto roll_gap_deg = current_roll * 180 / M_PI; yaw_gap_deg = yaw_gap * 180 / M_PI; pitch_gap_deg = pitch_gap * 180 / M_PI; - - RCLCPP_INFO(this->get_logger(), "current yaw: %f, pitch: %f", yaw_gap_deg, pitch_gap_deg); auto yaw = atan2(y, x); yaw = (x < 0) ? -yaw : yaw; auto degree = yaw * 180 / M_PI; - auto pitch = atan2(y, z); + auto x_y = sqrt(x * x + y * y); + auto pitch = atan2(x_y, z); pitch = (z < 0) ? -pitch : pitch; + pitch = pitch * 180 / M_PI; result->yaw_deg = static_cast(degree); result->pitch_deg = static_cast(pitch);