Define parameters for base_link, odom, map frames

mavros/include/mavros/mavros_uas.hpp

@@ -570,6 +570,10 @@ class UAS : public rclcpp::Node
 
   std::string uas_url;
 
+  std::string base_link_frame_id;
+  std::string odom_frame_id;
+  std::string map_frame_id;
+
   StrV plugin_allowlist;
   StrV plugin_denylist;
 

mavros/src/lib/mavros_uas.cpp

@@ -48,6 +48,9 @@ UAS::UAS(
   target_system(target_system_),
   target_component(target_component_),
   uas_url(uas_url_),
+  base_link_frame_id("base_link"),
+  odom_frame_id("odom"),
+  map_frame_id("map"),
   plugin_factory_loader("mavros", "mavros::plugin::PluginFactory"),
   loaded_plugins{},
   plugin_subscriptions{},
@@ -77,6 +80,9 @@ UAS::UAS(
   this->declare_parameter("target_component_id", target_component);
   this->declare_parameter("plugin_allowlist", plugin_allowlist);
   this->declare_parameter("plugin_denylist", plugin_denylist);
+  this->declare_parameter("base_link_frame_id", base_link_frame_id);
+  this->declare_parameter("odom_frame_id", odom_frame_id);
+  this->declare_parameter("map_frame_id", map_frame_id);
 
   // NOTE(vooon): we couldn't add_plugin() in constructor because it needs shared_from_this()
   startup_delay_timer = this->create_wall_timer(
@@ -93,6 +99,9 @@ UAS::UAS(
       this->get_parameter("target_component_id", tgt_component);
       this->get_parameter("plugin_allowlist", plugin_allowlist);
       this->get_parameter("plugin_denylist", plugin_denylist);
+      this->get_parameter("base_link_frame_id", base_link_frame_id);
+      this->get_parameter("odom_frame_id", odom_frame_id);
+      this->get_parameter("map_frame_id", map_frame_id);
 
       exec_spin_thd = thread_ptr(
         new std::thread(
@@ -151,21 +160,24 @@ UAS::UAS(
 
       // Publish helper TFs used for frame transformation in the odometry plugin
       {
+        std::string base_link_frd = base_link_frame_id+"_frd";
+        std::string odom_ned = odom_frame_id+"_ned";
+        std::string map_ned = map_frame_id+"_ned";
         std::vector<geometry_msgs::msg::TransformStamped> transform_vector;
         add_static_transform(
-          "map", "map_ned", Eigen::Affine3d(
+          map_frame_id, map_ned, Eigen::Affine3d(
             ftf::quaternion_from_rpy(
               M_PI, 0,
               M_PI_2)),
           transform_vector);
         add_static_transform(
-          "odom", "odom_ned", Eigen::Affine3d(
+          odom_frame_id, odom_ned, Eigen::Affine3d(
             ftf::quaternion_from_rpy(
               M_PI, 0,
               M_PI_2)),
           transform_vector);
         add_static_transform(
-          "base_link", "base_link_frd",
+          base_link_frame_id, base_link_frd,
           Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)), transform_vector);
 
         tf2_static_broadcaster.sendTransform(transform_vector);

mavros/src/mavros_node.cpp

@@ -32,6 +32,7 @@ int main(int argc, char * argv[])
   // options.use_intra_process_comms(true);
 
   std::string fcu_url, gcs_url, uas_url;
+  std::string base_link_frame_id, odom_frame_id, map_frame_id;
   int tgt_system = 1, tgt_component = 1;
 
   auto node = std::make_shared<rclcpp::Node>("mavros_node", options);
@@ -41,11 +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->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);
 
   uas_url = mavros::utils::format("/uas%d", tgt_system);
 
@@ -78,6 +85,24 @@ int main(int argc, char * argv[])
     tgt_component);
   exec.add_node(uas_node);
 
+  {
+    std::vector<rclcpp::Parameter> uas_params{};
+
+    if (base_link_frame_id != "") {
+      uas_params.emplace_back("base_link_frame_id", base_link_frame_id);
+    }
+
+    if (odom_frame_id != "") {
+      uas_params.emplace_back("odom_frame_id", odom_frame_id);
+    }
+
+    if (map_frame_id != "") {
+      uas_params.emplace_back("map_frame_id", map_frame_id);
+    }
+    uas_node->set_parameters(uas_params);
+
+  }
+
   exec.spin();
   rclcpp::shutdown();
   return 0;