updat auto-launch and tf-to-position

StrayedCats · Mar 11, 2024 · 11597c3 · 11597c3
1 parent 5106be4
commit 11597c3
Show file tree

Hide file tree

Showing 4 changed files with 17 additions and 2 deletions.
diff --git a/auto_driver_bringup/launch/auto2024.launch.py b/auto_driver_bringup/launch/auto2024.launch.py
@@ -33,8 +33,8 @@ def generate_launch_description():
                     parameters=[
                         {'p': 80},
                         {'i': 0},
-                        {'d': 10},
-                        {'max_spd': 8000},
+                        {'d': 5},
+                        {'max_spd': 3000},
                         {'init_deg': 120},
                         {'min_limit': 20},
                         {'max_limit': 135}

diff --git a/auto_driver_interface/include/auto_driver_interface/tf_to_position_server.hpp b/auto_driver_interface/include/auto_driver_interface/tf_to_position_server.hpp
@@ -23,6 +23,7 @@
 #include <rclcpp_action/rclcpp_action.hpp>
 #include <tf2_ros/transform_listener.h>
 #include <tf2_ros/buffer.h>
+#include <tf2/utils.h>
 
 #include "auto_driver_msgs/action/get_angles_from_tf.hpp"
 

diff --git a/auto_driver_interface/src/tf_to_position_server.cpp b/auto_driver_interface/src/tf_to_position_server.cpp
@@ -97,7 +97,17 @@ namespace auto_driver_interface
       auto x = transform2.transform.translation.x - transform.transform.translation.x;
       auto y = transform2.transform.translation.y - transform.transform.translation.y;
       auto z = transform2.transform.translation.z - transform.transform.translation.z;
+      auto current_rotation = transform.transform.rotation;
 
+      double yaw_gap, pitch_gap, current_roll, yaw_gap_deg, pitch_gap_deg;
+      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);
+      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;
@@ -107,6 +117,8 @@ namespace auto_driver_interface
 
       result->yaw_deg = static_cast<int32_t>(degree);
       result->pitch_deg = static_cast<int32_t>(pitch);
+      result->yaw_gap_deg = static_cast<int32_t>(yaw_gap_deg);
+      result->pitch_gap_deg = static_cast<int32_t>(pitch_gap_deg);
       result->succeed = true;
 
       RCLCPP_INFO(this->get_logger(), "yaw: %f", degree);

diff --git a/auto_driver_msgs/action/GetAnglesFromTf.action b/auto_driver_msgs/action/GetAnglesFromTf.action
@@ -7,5 +7,7 @@ string target_tf_frame_id
 bool succeed
 int32 yaw_deg
 int32 pitch_deg
+int32 yaw_gap_deg
+int32 pitch_gap_deg
 ---
 #feedback definition