Define _frd frames in odom plugin based on parent/child frame parametrs

mavlink · Oct 11, 2023 · 3b3804f · 3b3804f
1 parent a54cb99
commit 3b3804f
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 10 deletions.
diff --git 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
@@ -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;