From 7d5b216ab98156e7be1f7c75732c7f32149bf63a Mon Sep 17 00:00:00 2001 From: Dongmyeong Lee Date: Fri, 3 Nov 2023 20:13:54 -0500 Subject: [PATCH 1/2] Add fallback when TF tree for laserscan is not published --- src/navigation/navigation_main.cc | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index 8d61272..bf77716 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -212,7 +212,12 @@ void RetrieveTransform(const std_msgs::Header& msg, transform.getRotation().getY(), transform.getRotation().getZ()); } catch (tf::TransformException& ex) { - ROS_ERROR("%s", ex.what()); + ROS_WARN("Failed to retrieve transform from '%s' to '%s': %s", + msg.frame_id.c_str(), CONFIG_laser_frame.c_str(), ex.what()); + ROS_INFO("Assuming '%s' frame is equal to '%s' frame;", + msg.frame_id.c_str(), CONFIG_laser_frame.c_str()); + // Set frame_tf to identity as a fallback. + frame_tf = Eigen::Affine3f::Identity(); } } From d01b0e471d74c732ec540f0b728553a27da0639d Mon Sep 17 00:00:00 2001 From: Dongmyeong Lee Date: Fri, 3 Nov 2023 23:57:54 -0500 Subject: [PATCH 2/2] reduce wait time duration for tf_listner --- src/navigation/navigation_main.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/navigation/navigation_main.cc b/src/navigation/navigation_main.cc index bf77716..9f35cb5 100644 --- a/src/navigation/navigation_main.cc +++ b/src/navigation/navigation_main.cc @@ -95,6 +95,7 @@ using std::unordered_map; using sensor_msgs::PointCloud; using Eigen::Vector2f; using Eigen::Vector3f; +using Eigen::Affine3f; using graph_navigation::graphNavSrv; using geometry_msgs::TwistStamped; using geometry::kEpsilon; @@ -143,6 +144,7 @@ struct LaserCache { float dtheta = 0.0f; float angle_min = 0.0f; vector rays; + Affine3f frame_tf = Affine3f::Identity(); }; // Publishers @@ -199,7 +201,7 @@ void RetrieveTransform(const std_msgs::Header& msg, tf_listener.waitForTransform(CONFIG_laser_frame, msg.frame_id, msg.stamp, - ros::Duration(0.1)); + ros::Duration(0.01)); tf_listener.lookupTransform(CONFIG_laser_frame, msg.frame_id, msg.stamp, @@ -214,10 +216,8 @@ void RetrieveTransform(const std_msgs::Header& msg, } catch (tf::TransformException& ex) { ROS_WARN("Failed to retrieve transform from '%s' to '%s': %s", msg.frame_id.c_str(), CONFIG_laser_frame.c_str(), ex.what()); - ROS_INFO("Assuming '%s' frame is equal to '%s' frame;", - msg.frame_id.c_str(), CONFIG_laser_frame.c_str()); - // Set frame_tf to identity as a fallback. - frame_tf = Eigen::Affine3f::Identity(); + // Use previous transform (Identity if this is the first message) + ROS_INFO("Use previous transform"); } } @@ -248,7 +248,7 @@ void LaserHandler(const sensor_msgs::LaserScan& msg, CHECK_EQ(cache.rays.size(), msg.ranges.size()); // Lookup tf transform from laser frame to base frame. - Eigen::Affine3f frame_tf; + Affine3f& frame_tf = cache.frame_tf; RetrieveTransform(msg.header, frame_tf); size_t start_idx = point_cloud_.size();