Skip to content

Commit

Permalink
add frame_id parameter
Browse files Browse the repository at this point in the history
  • Loading branch information
@EnderMandS @vooon
EnderMandS authored and vooon committed Jul 16, 2024
1 parent a71060b commit ce01a6d
Show file tree
Hide file tree
Showing 5 changed files with 64 additions and 21 deletions.
22 changes: 22 additions & 0 deletions mavros/include/mavros/mavros_uas.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand Down Expand Up @@ -472,5 +492,7 @@ class UAS {

std::atomic<bool> fcu_caps_known;
std::atomic<uint64_t> fcu_capabilities;

std::string base_link_frame_id, odom_frame_id, map_frame_id;
};
} // namespace mavros
13 changes: 13 additions & 0 deletions mavros/src/lib/mavros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string>("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<std::string>("odom_frame_id", odom_frame_id, "odom"))
ROS_INFO("Find param odom_frame_id: %s", odom_frame_id.c_str());
if (nh.param<std::string>("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
Expand Down
23 changes: 14 additions & 9 deletions mavros/src/lib/uas_data.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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<geometry_msgs::TransformStamped> 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 -*- */
Expand Down Expand Up @@ -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<geometry_msgs::TransformStamped> 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);
}
19 changes: 11 additions & 8 deletions mavros_extras/src/plugins/odom.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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")
{ }

Expand All @@ -56,8 +57,9 @@ class OdometryPlugin : public plugin::PluginBase {
PluginBase::initialize(uas_);

// frame params:
odom_nh.param<std::string>("fcu/odom_parent_id_des", fcu_odom_parent_id_des, "map");
odom_nh.param<std::string>("fcu/odom_child_id_des", fcu_odom_child_id_des, "base_link");
odom_nh.param<std::string>("fcu/odom_parent_id_des", fcu_map_id_des, uas_.get_map_frame_id());
odom_nh.param<std::string>("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<nav_msgs::Odometry>("in", 10);
Expand All @@ -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
Expand Down Expand Up @@ -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();
Expand All @@ -142,7 +145,7 @@ class OdometryPlugin : public plugin::PluginBase {

auto odom = boost::make_shared<nav_msgs::Odometry>();

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;

/**
Expand Down Expand Up @@ -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;
Expand Down
8 changes: 4 additions & 4 deletions mavros_extras/src/plugins/wheel_odometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,14 +69,14 @@ class WheelOdometryPlugin : public plugin::PluginBase {

// Odometry params
wo_nh.param("send_twist", twist_send, false);
wo_nh.param<std::string>("frame_id", frame_id, "odom");
wo_nh.param<std::string>("child_frame_id", child_frame_id, "base_link");
wo_nh.param<std::string>("frame_id", frame_id, uas_.get_odom_frame_id());
wo_nh.param<std::string>("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<std::string>("tf/frame_id", tf_frame_id, "odom");
wo_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");
wo_nh.param<std::string>("tf/frame_id", tf_frame_id, uas_.get_odom_frame_id());
wo_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, uas_.get_base_link_frame_id());

// Read parameters for each wheel.
{
Expand Down

0 comments on commit ce01a6d

Please sign in to comment.