From 31aff6b60e1cb6fea3569030225134bedb81a598 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Mon, 23 Oct 2023 09:41:25 +0200 Subject: [PATCH 1/4] increase max covariance --- imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg b/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg index 86cbd35f..461cebee 100755 --- a/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg +++ b/imu_filter_madgwick/cfg/ImuFilterMadgwick.cfg @@ -12,6 +12,6 @@ gen.add("zeta", double_t, 0, "Gyro drift gain (approx. rad/s).", 0, -1.0, 1.0) gen.add("mag_bias_x", double_t, 0, "Magnetometer bias (hard iron correction), x component.", 0, -10.0, 10.0) gen.add("mag_bias_y", double_t, 0, "Magnetometer bias (hard iron correction), y component.", 0, -10.0, 10.0) gen.add("mag_bias_z", double_t, 0, "Magnetometer bias (hard iron correction), z component.", 0, -10.0, 10.0) -gen.add("orientation_stddev", double_t, 0, "Standard deviation of the orientation estimate.", 0, 0, 1.0) +gen.add("orientation_stddev", double_t, 0, "Standard deviation of the orientation estimate.", 0, 0, 100.0) exit(gen.generate(PACKAGE, "dynamic_reconfigure_node", "ImuFilterMadgwick")) From ccffa44e018d12921bfd4fbad4e9052189cee35a Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 17 Oct 2023 14:53:50 +0200 Subject: [PATCH 2/4] show correct topics even if remapped --- imu_filter_madgwick/src/imu_filter_ros.cpp | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index a68f1a2f..5a7a652c 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -445,13 +445,11 @@ void ImuFilterRos::checkTopicsTimerCallback(const ros::TimerEvent&) { if (use_mag_) ROS_WARN_STREAM("Still waiting for data on topics " - << ros::names::resolve("imu") << "/data_raw" - << " and " << ros::names::resolve("imu") << "/mag" - << "..."); + << imu_subscriber_->getTopic() << " and " + << mag_subscriber_->getTopic() << "..."); else ROS_WARN_STREAM("Still waiting for data on topic " - << ros::names::resolve("imu") << "/data_raw" - << "..."); + << imu_subscriber_->getTopic() << "..."); } void ImuFilterRos::reset() From e90693b3a90b11654d6e2c82c53f958687fdc7bb Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Tue, 17 Oct 2023 14:18:26 +0200 Subject: [PATCH 3/4] publish orientation as Pose to vis on rviz --- .../imu_filter_madgwick/imu_filter_ros.h | 6 +++ imu_filter_madgwick/src/imu_filter_ros.cpp | 38 ++++++++++++++++++- 2 files changed, 43 insertions(+), 1 deletion(-) diff --git a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h index 9276b4fe..5adee06a 100644 --- a/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h +++ b/imu_filter_madgwick/include/imu_filter_madgwick/imu_filter_ros.h @@ -29,7 +29,9 @@ #include #include #include +#include #include "tf2_ros/transform_broadcaster.h" +#include #include #include #include @@ -74,8 +76,11 @@ class ImuFilterRos ros::Publisher rpy_filtered_debug_publisher_; ros::Publisher rpy_raw_debug_publisher_; + ros::Publisher orientation_filtered_publisher_; ros::Publisher imu_publisher_; tf2_ros::TransformBroadcaster tf_broadcaster_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; boost::shared_ptr config_server_; ros::Timer check_topics_timer_; @@ -111,6 +116,7 @@ class ImuFilterRos void imuCallback(const ImuMsg::ConstPtr& imu_msg_raw); void publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw); + void publishOrientationFiltered(const ImuMsg::ConstPtr& imu_msg); void publishTransform(const ImuMsg::ConstPtr& imu_msg_raw); void publishRawMsg(const ros::Time& t, float roll, float pitch, float yaw); diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index 5a7a652c..19741e58 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -25,11 +25,15 @@ #include "imu_filter_madgwick/imu_filter_ros.h" #include "imu_filter_madgwick/stateless_orientation.h" #include "geometry_msgs/TransformStamped.h" +#include "geometry_msgs/PoseStamped.h" #include #include ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private) - : nh_(nh), nh_private_(nh_private), initialized_(false) + : nh_(nh), + nh_private_(nh_private), + initialized_(false), + tf_listener_(tf_buffer_) { ROS_INFO("Starting ImuFilter"); @@ -121,6 +125,10 @@ ImuFilterRos::ImuFilterRos(ros::NodeHandle nh, ros::NodeHandle nh_private) rpy_raw_debug_publisher_ = nh_.advertise( ros::names::resolve("imu") + "/rpy/raw", 5); + + orientation_filtered_publisher_ = + nh_private.advertise( + ros::names::resolve("imu") + "/orientation_filtered", 5); } // **** register subscribers @@ -407,9 +415,37 @@ void ImuFilterRos::publishFilteredMsg(const ImuMsg::ConstPtr& imu_msg_raw) rpy.header = imu_msg_raw->header; rpy_filtered_debug_publisher_.publish(rpy); + + publishOrientationFiltered(imu_msg); } } +void ImuFilterRos::publishOrientationFiltered(const ImuMsg::ConstPtr& imu_msg) +{ + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.stamp = imu_msg->header.stamp; + pose_msg.header.frame_id = fixed_frame_; + pose_msg.pose.orientation = imu_msg->orientation; + + // get the current transform from the fixed frame to the imu frame + geometry_msgs::TransformStamped transform; + try + { + transform = tf_buffer_.lookupTransform( + fixed_frame_, imu_msg->header.frame_id, imu_msg->header.stamp); + } catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return; + } + + pose_msg.pose.position.x = transform.transform.translation.x; + pose_msg.pose.position.y = transform.transform.translation.y; + pose_msg.pose.position.z = transform.transform.translation.z; + + orientation_filtered_publisher_.publish(pose_msg); +} + void ImuFilterRos::publishRawMsg(const ros::Time& t, float roll, float pitch, float yaw) { From 25f310d6f427d993db30f37da773aee7d607b0c3 Mon Sep 17 00:00:00 2001 From: Adi Vardi Date: Wed, 1 Nov 2023 11:59:39 +0100 Subject: [PATCH 4/4] add timeout to lookupTransform Otherwise there are a lot of warnings of `lookup would require extrapolation into the future` --- imu_filter_madgwick/src/imu_filter_ros.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/imu_filter_madgwick/src/imu_filter_ros.cpp b/imu_filter_madgwick/src/imu_filter_ros.cpp index 19741e58..ed8bd982 100644 --- a/imu_filter_madgwick/src/imu_filter_ros.cpp +++ b/imu_filter_madgwick/src/imu_filter_ros.cpp @@ -432,7 +432,8 @@ void ImuFilterRos::publishOrientationFiltered(const ImuMsg::ConstPtr& imu_msg) try { transform = tf_buffer_.lookupTransform( - fixed_frame_, imu_msg->header.frame_id, imu_msg->header.stamp); + fixed_frame_, imu_msg->header.frame_id, imu_msg->header.stamp, + ros::Duration(0.1)); } catch (tf2::TransformException& ex) { ROS_WARN("%s", ex.what());