From ce01a6ddf9fe03ea6ad37016a938c9e19a302d24 Mon Sep 17 00:00:00 2001 From: EnderMandS <952025764@qq.com> Date: Mon, 15 Jul 2024 16:28:31 +0800 Subject: [PATCH] add frame_id parameter --- mavros/include/mavros/mavros_uas.h | 22 +++++++++++++++++++ mavros/src/lib/mavros.cpp | 13 +++++++++++ mavros/src/lib/uas_data.cpp | 23 ++++++++++++-------- mavros_extras/src/plugins/odom.cpp | 19 +++++++++------- mavros_extras/src/plugins/wheel_odometry.cpp | 8 +++---- 5 files changed, 64 insertions(+), 21 deletions(-) diff --git a/mavros/include/mavros/mavros_uas.h b/mavros/include/mavros/mavros_uas.h index d6d9044ac..feff9cd84 100644 --- a/mavros/include/mavros/mavros_uas.h +++ b/mavros/include/mavros/mavros_uas.h @@ -444,6 +444,26 @@ class UAS { */ bool cmode_from_str(std::string cmode_str, uint32_t &custom_mode); + inline void set_base_link_frame_id(const std::string frame_id) { + base_link_frame_id = frame_id; + } + inline void set_odom_frame_id(const std::string frame_id) { + odom_frame_id = frame_id; + } + inline void set_map_frame_id(const std::string frame_id) { + map_frame_id = frame_id; + } + inline std::string get_base_link_frame_id() { + return base_link_frame_id; + } + inline std::string get_odom_frame_id() { + return odom_frame_id; + } + inline std::string get_map_frame_id() { + return map_frame_id; + } + void setup_static_tf(); + private: std::recursive_mutex mutex; @@ -472,5 +492,7 @@ class UAS { std::atomic fcu_caps_known; std::atomic fcu_capabilities; + + std::string base_link_frame_id, odom_frame_id, map_frame_id; }; } // namespace mavros diff --git a/mavros/src/lib/mavros.cpp b/mavros/src/lib/mavros.cpp index aff0987e6..359d782d9 100644 --- a/mavros/src/lib/mavros.cpp +++ b/mavros/src/lib/mavros.cpp @@ -64,6 +64,19 @@ MavRos::MavRos() : nh.getParam("plugin_blacklist", plugin_blacklist); nh.getParam("plugin_whitelist", plugin_whitelist); + // Get frame_id + std::string base_link_frame_id, odom_frame_id, map_frame_id; + if (nh.param("base_link_frame_id", base_link_frame_id, "base_link")) + ROS_INFO("Find param base_link_frame_id: %s", base_link_frame_id.c_str()); + if (nh.param("odom_frame_id", odom_frame_id, "odom")) + ROS_INFO("Find param odom_frame_id: %s", odom_frame_id.c_str()); + if (nh.param("map_frame_id", map_frame_id, "map")) + ROS_INFO("Find param map_frame_id: %s", map_frame_id.c_str()); + mav_uas.set_base_link_frame_id(base_link_frame_id); + mav_uas.set_odom_frame_id(odom_frame_id); + mav_uas.set_map_frame_id(map_frame_id); + mav_uas.setup_static_tf(); + conn_timeout = ros::Duration(conn_timeout_d); // Now we use FCU URL as a hardware Id diff --git a/mavros/src/lib/uas_data.cpp b/mavros/src/lib/uas_data.cpp index 6dcfa5604..4e2282736 100644 --- a/mavros/src/lib/uas_data.cpp +++ b/mavros/src/lib/uas_data.cpp @@ -36,7 +36,10 @@ UAS::UAS() : time_offset(0), tsync_mode(UAS::timesync_mode::NONE), fcu_caps_known(false), - fcu_capabilities(0) + fcu_capabilities(0), + base_link_frame_id("base_link"), + odom_frame_id("odom"), + map_frame_id("map") { try { // Using smallest dataset with 5' grid, @@ -50,14 +53,6 @@ UAS::UAS() : " | Run install_geographiclib_dataset.sh script in order to install Geoid Model dataset!"); ros::shutdown(); } - - // Publish helper TFs used for frame transformation in the odometry plugin - std::vector transform_vector; - add_static_transform("map", "map_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector); - add_static_transform("odom", "odom_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector); - add_static_transform("base_link", "base_link_frd", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)),transform_vector); - - tf2_static_broadcaster.sendTransform(transform_vector); } /* -*- heartbeat handlers -*- */ @@ -275,3 +270,13 @@ void UAS::publish_static_transform(const std::string &frame_id, const std::strin tf2_static_broadcaster.sendTransform(static_transformStamped); } + +//! Publish helper TFs used for frame transformation in the odometry plugin +void UAS::setup_static_tf() +{ + std::vector transform_vector; + add_static_transform(map_frame_id, map_frame_id+"_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector); + add_static_transform(odom_frame_id, odom_frame_id+"_ned", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, M_PI_2)),transform_vector); + add_static_transform(base_link_frame_id, base_link_frame_id+"_frd", Eigen::Affine3d(ftf::quaternion_from_rpy(M_PI, 0, 0)),transform_vector); + tf2_static_broadcaster.sendTransform(transform_vector); +} diff --git a/mavros_extras/src/plugins/odom.cpp b/mavros_extras/src/plugins/odom.cpp index aa0e81f0a..fed8b31be 100644 --- a/mavros_extras/src/plugins/odom.cpp +++ b/mavros_extras/src/plugins/odom.cpp @@ -47,7 +47,8 @@ class OdometryPlugin : public plugin::PluginBase { OdometryPlugin() : PluginBase(), odom_nh("~odometry"), - fcu_odom_parent_id_des("map"), + fcu_map_id_des("map"), + fcu_odom_parent_id_des("odom"), fcu_odom_child_id_des("base_link") { } @@ -56,8 +57,9 @@ class OdometryPlugin : public plugin::PluginBase { PluginBase::initialize(uas_); // frame params: - odom_nh.param("fcu/odom_parent_id_des", fcu_odom_parent_id_des, "map"); - odom_nh.param("fcu/odom_child_id_des", fcu_odom_child_id_des, "base_link"); + odom_nh.param("fcu/odom_parent_id_des", fcu_map_id_des, uas_.get_map_frame_id()); + odom_nh.param("fcu/odom_child_id_des", fcu_odom_child_id_des, uas_.get_base_link_frame_id()); + fcu_odom_parent_id_des = uas_.get_odom_frame_id(); // publishers odom_pub = odom_nh.advertise("in", 10); @@ -80,6 +82,7 @@ class OdometryPlugin : public plugin::PluginBase { std::string fcu_odom_parent_id_des; //!< desired orientation of the fcu odometry message's parent frame std::string fcu_odom_child_id_des; //!< desired orientation of the fcu odometry message's child frame + std::string fcu_map_id_des; //!< desired orientation of the fcu map frame /** * @brief Lookup static transform with error handling @@ -122,8 +125,8 @@ class OdometryPlugin : public plugin::PluginBase { 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_map_id_des, fcu_map_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(); @@ -142,7 +145,7 @@ class OdometryPlugin : public plugin::PluginBase { auto odom = boost::make_shared(); - odom->header = m_uas->synchronized_header(fcu_odom_parent_id_des, odom_msg.time_usec); + odom->header = m_uas->synchronized_header(fcu_map_id_des, odom_msg.time_usec); odom->child_frame_id = fcu_odom_child_id_des; /** @@ -203,8 +206,8 @@ class OdometryPlugin : public plugin::PluginBase { 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(fcu_odom_parent_id_des+"_ned", odom->header.frame_id, tf_parent2parent_des); + lookup_static_transform(fcu_odom_child_id_des+"_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; diff --git a/mavros_extras/src/plugins/wheel_odometry.cpp b/mavros_extras/src/plugins/wheel_odometry.cpp index 22e8e1232..d754a1ba9 100644 --- a/mavros_extras/src/plugins/wheel_odometry.cpp +++ b/mavros_extras/src/plugins/wheel_odometry.cpp @@ -69,14 +69,14 @@ class WheelOdometryPlugin : public plugin::PluginBase { // Odometry params wo_nh.param("send_twist", twist_send, false); - wo_nh.param("frame_id", frame_id, "odom"); - wo_nh.param("child_frame_id", child_frame_id, "base_link"); + wo_nh.param("frame_id", frame_id, uas_.get_odom_frame_id()); + wo_nh.param("child_frame_id", child_frame_id, uas_.get_base_link_frame_id()); wo_nh.param("vel_error", vel_cov, 0.1); vel_cov = vel_cov * vel_cov; // std -> cov // TF subsection wo_nh.param("tf/send", tf_send, false); - wo_nh.param("tf/frame_id", tf_frame_id, "odom"); - wo_nh.param("tf/child_frame_id", tf_child_frame_id, "base_link"); + wo_nh.param("tf/frame_id", tf_frame_id, uas_.get_odom_frame_id()); + wo_nh.param("tf/child_frame_id", tf_child_frame_id, uas_.get_base_link_frame_id()); // Read parameters for each wheel. {