From 3b3804f6a73eee746d85798274238406c5e3f0a4 Mon Sep 17 00:00:00 2001 From: Mohamed Abdelkader Date: Wed, 11 Oct 2023 23:00:59 +0300 Subject: [PATCH] Define _frd frames in odom plugin based on parent/child frame parametrs --- mavros/src/mavros_node.cpp | 12 ++++++------ mavros_extras/src/plugins/odom.cpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/mavros/src/mavros_node.cpp b/mavros/src/mavros_node.cpp index 44edf53af..495220351 100644 --- a/mavros/src/mavros_node.cpp +++ b/mavros/src/mavros_node.cpp @@ -42,17 +42,17 @@ int main(int argc, char * argv[]) node->declare_parameter("gcs_url", gcs_url); node->declare_parameter("tgt_system", tgt_system); node->declare_parameter("tgt_component", tgt_component); - node->declare_parameter("base_link_frame_id", base_link_frame_id); - node->declare_parameter("map_frame_id", map_frame_id); - node->declare_parameter("odom_frame_id", odom_frame_id); + node->declare_parameter("base_link_frame", base_link_frame_id); + node->declare_parameter("map_frame", map_frame_id); + node->declare_parameter("odom_frame", odom_frame_id); node->get_parameter("fcu_url", fcu_url); node->get_parameter("gcs_url", gcs_url); node->get_parameter("tgt_system", tgt_system); node->get_parameter("tgt_component", tgt_component); - node->get_parameter("base_link_frame_id", base_link_frame_id); - node->get_parameter("map_frame_id", map_frame_id); - node->get_parameter("odom_frame_id", odom_frame_id); + node->get_parameter("base_link_frame", base_link_frame_id); + node->get_parameter("map_frame", map_frame_id); + node->get_parameter("odom_frame", odom_frame_id); uas_url = mavros::utils::format("/uas%d", tgt_system); diff --git a/mavros_extras/src/plugins/odom.cpp b/mavros_extras/src/plugins/odom.cpp index 856a56673..c1502c977 100644 --- a/mavros_extras/src/plugins/odom.cpp +++ b/mavros_extras/src/plugins/odom.cpp @@ -144,8 +144,8 @@ class OdometryPlugin : public plugin::Plugin Eigen::Affine3d tf_parent2parent_des; Eigen::Affine3d tf_child2child_des; - lookup_static_transform(fcu_odom_parent_id_des, "map_ned", tf_parent2parent_des); - lookup_static_transform(fcu_odom_child_id_des, "base_link_frd", tf_child2child_des); + lookup_static_transform(fcu_odom_parent_id_des, fcu_odom_parent_id_des+"_ned", tf_parent2parent_des); + lookup_static_transform(fcu_odom_child_id_des, fcu_odom_child_id_des+"_frd", tf_child2child_des); //! Build 6x6 pose covariance matrix to be transformed and sent Matrix6d cov_pose = Matrix6d::Zero(); @@ -235,8 +235,8 @@ class OdometryPlugin : public plugin::Plugin Eigen::Affine3d tf_parent2parent_des; Eigen::Affine3d tf_child2child_des; - lookup_static_transform("odom_ned", odom->header.frame_id, tf_parent2parent_des); - lookup_static_transform("base_link_frd", odom->child_frame_id, tf_child2child_des); + lookup_static_transform(odom->header.frame_id+"_ned", odom->header.frame_id, tf_parent2parent_des); + lookup_static_transform(odom->child_frame_id+"_frd", odom->child_frame_id, tf_child2child_des); //! Build 6x6 pose covariance matrix to be transformed and sent ftf::Covariance6d cov_pose = odom->pose.covariance;