diff --git a/dependencies.rosinstall b/dependencies.rosinstall index b1b701537..ddbb5be35 100644 --- a/dependencies.rosinstall +++ b/dependencies.rosinstall @@ -1,4 +1,4 @@ - git: local-name: mavlink uri: https://github.com/mavlink/mavlink-gbp-release.git - version: release/kinetic/mavlink + version: release/noetic/mavlink diff --git a/libmavconn/CHANGELOG.rst b/libmavconn/CHANGELOG.rst index f22006f01..0dc0be05a 100644 --- a/libmavconn/CHANGELOG.rst +++ b/libmavconn/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package libmavconn ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.20.0 (2024-10-10) +------------------- + 1.19.0 (2024-06-06) ------------------- diff --git a/libmavconn/package.xml b/libmavconn/package.xml index 641541e67..24538e83d 100644 --- a/libmavconn/package.xml +++ b/libmavconn/package.xml @@ -1,7 +1,7 @@ libmavconn - 1.19.0 + 1.20.0 MAVLink communication library. This library provide unified connection handling classes diff --git a/mavros/CHANGELOG.rst b/mavros/CHANGELOG.rst index a7475f88c..966fb6132 100644 --- a/mavros/CHANGELOG.rst +++ b/mavros/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mavros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.20.0 (2024-10-10) +------------------- +* add param to odom plugin +* add frame_id parameter +* Contributors: EnderMandS + 1.19.0 (2024-06-06) ------------------- * gps_global_origin: remove LLA to ECEF conversion 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/launch/px4_config.yaml b/mavros/launch/px4_config.yaml index 6b2913c94..bb18b088f 100644 --- a/mavros/launch/px4_config.yaml +++ b/mavros/launch/px4_config.yaml @@ -231,8 +231,9 @@ mount: # odom odometry: fcu: - odom_parent_id_des: "map" # desired parent frame rotation of the FCU's odometry + odom_parent_id_des: "odom" # desired parent frame rotation of the FCU's odometry odom_child_id_des: "base_link" # desired child frame rotation of the FCU's odometry + map_id_des: "map" # px4flow px4flow: diff --git a/mavros/package.xml b/mavros/package.xml index d36fdd58d..8b454f97a 100644 --- a/mavros/package.xml +++ b/mavros/package.xml @@ -1,7 +1,7 @@ mavros - 1.19.0 + 1.20.0 MAVROS -- MAVLink extendable communication node for ROS with proxy for Ground Control Station. 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/CHANGELOG.rst b/mavros_extras/CHANGELOG.rst index df1e9245b..e78c35a4a 100644 --- a/mavros_extras/CHANGELOG.rst +++ b/mavros_extras/CHANGELOG.rst @@ -2,6 +2,16 @@ Changelog for package mavros_extras ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.20.0 (2024-10-10) +------------------- +* Add missing std_srvs dependency +* add param to odom plugin +* add frame_id parameter +* Fix compile error when compiling with gcc 13 + The error is: + src/plugins/mag_calibration_status.cpp:64:22: error: ‘bitset’ is not a member of ‘std’ +* Contributors: EnderMandS, Michal Sojka, Roland Arsenault + 1.19.0 (2024-06-06) ------------------- diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index 26e3a71bd..084819a1e 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs std_msgs + std_srvs tf tf2_eigen urdf diff --git a/mavros_extras/package.xml b/mavros_extras/package.xml index 18c7a39aa..bb8a649b3 100644 --- a/mavros_extras/package.xml +++ b/mavros_extras/package.xml @@ -1,7 +1,7 @@ mavros_extras - 1.19.0 + 1.20.0 Extra nodes and plugins for MAVROS. @@ -26,6 +26,7 @@ mavros_msgs sensor_msgs std_msgs + std_srvs visualization_msgs urdf tf diff --git a/mavros_extras/src/plugins/mag_calibration_status.cpp b/mavros_extras/src/plugins/mag_calibration_status.cpp index 70c9d15de..69a735cb4 100644 --- a/mavros_extras/src/plugins/mag_calibration_status.cpp +++ b/mavros_extras/src/plugins/mag_calibration_status.cpp @@ -11,6 +11,8 @@ #include #include #include +#include + namespace mavros { namespace std_plugins { /** diff --git a/mavros_extras/src/plugins/odom.cpp b/mavros_extras/src/plugins/odom.cpp index aa0e81f0a..44b8f4621 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_odom_parent_id_des, uas_.get_odom_frame_id()); + odom_nh.param("fcu/odom_child_id_des", fcu_odom_child_id_des, uas_.get_base_link_frame_id()); + odom_nh.param("fcu/map_id_des", fcu_map_id_des, uas_.get_map_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. { diff --git a/mavros_msgs/CHANGELOG.rst b/mavros_msgs/CHANGELOG.rst index a1790fa83..741e1ff77 100644 --- a/mavros_msgs/CHANGELOG.rst +++ b/mavros_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mavros_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.20.0 (2024-10-10) +------------------- + 1.19.0 (2024-06-06) ------------------- diff --git a/mavros_msgs/package.xml b/mavros_msgs/package.xml index d83207030..9e8006894 100644 --- a/mavros_msgs/package.xml +++ b/mavros_msgs/package.xml @@ -1,7 +1,7 @@ mavros_msgs - 1.19.0 + 1.20.0 mavros_msgs defines messages for MAVROS. diff --git a/test_mavros/CHANGELOG.rst b/test_mavros/CHANGELOG.rst index c456ccfda..192a5c537 100644 --- a/test_mavros/CHANGELOG.rst +++ b/test_mavros/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package test_mavros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.20.0 (2024-10-10) +------------------- + 1.19.0 (2024-06-06) ------------------- diff --git a/test_mavros/package.xml b/test_mavros/package.xml index df4ba9c74..a83a8680e 100644 --- a/test_mavros/package.xml +++ b/test_mavros/package.xml @@ -1,7 +1,7 @@ test_mavros - 1.19.0 + 1.20.0 Tests for MAVROS package Vladimir Ermakov