diff --git a/auto_driver_interface/src/tf_to_position_server.cpp b/auto_driver_interface/src/tf_to_position_server.cpp index 7060396..7a48634 100644 --- a/auto_driver_interface/src/tf_to_position_server.cpp +++ b/auto_driver_interface/src/tf_to_position_server.cpp @@ -99,14 +99,6 @@ namespace auto_driver_interface 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); - 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; - auto yaw = atan2(y, x); yaw = (x < 0) ? -yaw : yaw; auto degree = yaw * 180 / M_PI; @@ -118,12 +110,8 @@ namespace auto_driver_interface result->yaw_deg = static_cast(degree); result->pitch_deg = static_cast(pitch); - result->yaw_gap_deg = static_cast(yaw_gap_deg); - result->pitch_gap_deg = static_cast(pitch_gap_deg); result->succeed = true; - RCLCPP_INFO(this->get_logger(), "yaw: %f", degree); - goal_handle->succeed(result); return; } diff --git a/auto_driver_msgs/action/GetAnglesFromTf.action b/auto_driver_msgs/action/GetAnglesFromTf.action index ed97eab..dc3d306 100644 --- a/auto_driver_msgs/action/GetAnglesFromTf.action +++ b/auto_driver_msgs/action/GetAnglesFromTf.action @@ -7,7 +7,5 @@ string target_tf_frame_id bool succeed int32 yaw_deg int32 pitch_deg -int32 yaw_gap_deg -int32 pitch_gap_deg --- #feedback definition \ No newline at end of file