Feng/ros2 (#17)

* Fix empty TF problem * Reduce busy loop --------- Co-authored-by: Zhe Feng <[email protected]>
fixposition · Apr 19, 2023 · 28b01c7 · 28b01c7
1 parent 12ca10d
commit 28b01c7
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 21 deletions.
diff --git a/fixposition_driver_ros1/src/fixposition_driver_node.cpp b/fixposition_driver_ros1/src/fixposition_driver_node.cpp
@@ -87,16 +87,18 @@ void FixpositionDriverNode::RegisterObservers() {
                     }
 
                     // TFs
-                    geometry_msgs::TransformStamped tf_ecef_poi;
-                    geometry_msgs::TransformStamped tf_ecef_enu;
-                    geometry_msgs::TransformStamped tf_ecef_enu0;
-                    TfDataToMsg(data.tf_ecef_poi, tf_ecef_poi);
-                    TfDataToMsg(data.tf_ecef_enu, tf_ecef_enu);
-                    TfDataToMsg(data.tf_ecef_enu0, tf_ecef_enu0);
-
-                    br_.sendTransform(tf_ecef_enu);
-                    br_.sendTransform(tf_ecef_poi);
-                    static_br_.sendTransform(tf_ecef_enu0);
+                    if (data.vrtk.fusion_status > 0) {
+                        geometry_msgs::TransformStamped tf_ecef_poi;
+                        geometry_msgs::TransformStamped tf_ecef_enu;
+                        geometry_msgs::TransformStamped tf_ecef_enu0;
+                        TfDataToMsg(data.tf_ecef_poi, tf_ecef_poi);
+                        TfDataToMsg(data.tf_ecef_enu, tf_ecef_enu);
+                        TfDataToMsg(data.tf_ecef_enu0, tf_ecef_enu0);
+
+                        br_.sendTransform(tf_ecef_enu);
+                        br_.sendTransform(tf_ecef_poi);
+                        static_br_.sendTransform(tf_ecef_enu0);
+                    }
                 });
         } else if (format == "LLH" && converters_["LLH"]) {
             dynamic_cast<LlhConverter*>(converters_["LLH"].get())->AddObserver([this](const NavSatFixData& data) {
@@ -154,6 +156,8 @@ void FixpositionDriverNode::Run() {
             printf("Reconnecting in %.1f seconds ...\n", params_.fp_output.reconnect_delay);
             ros::Duration(params_.fp_output.reconnect_delay).sleep();
             Connect();
+        } else {
+            rate.sleep(); //ensure the loop runs at the desired rate
         }
     }
 }

diff --git a/fixposition_driver_ros2/src/fixposition_driver_node.cpp b/fixposition_driver_ros2/src/fixposition_driver_node.cpp
@@ -55,7 +55,7 @@ FixpositionDriverNode::FixpositionDriverNode(std::shared_ptr<rclcpp::Node> node,
 }
 
 void FixpositionDriverNode::Run() {
-    const rclcpp::Rate rate(params_.fp_output.rate);
+    rclcpp::Rate rate(params_.fp_output.rate);
     const auto reconnect_delay =
         std::chrono::nanoseconds((uint64_t)params_.fp_output.reconnect_delay * 1000 * 1000 * 1000);
 
@@ -70,6 +70,8 @@ void FixpositionDriverNode::Run() {
 
             rclcpp::sleep_for(reconnect_delay);
             Connect();
+        } else {
+            rate.sleep();
         }
     }
 }
@@ -113,16 +115,18 @@ void FixpositionDriverNode::RegisterObservers() {
                     }
 
                     // TFs
-                    geometry_msgs::msg::TransformStamped tf_ecef_poi;
-                    geometry_msgs::msg::TransformStamped tf_ecef_enu;
-                    geometry_msgs::msg::TransformStamped tf_ecef_enu0;
-                    TfDataToMsg(data.tf_ecef_poi, tf_ecef_poi);
-                    TfDataToMsg(data.tf_ecef_enu, tf_ecef_enu);
-                    TfDataToMsg(data.tf_ecef_enu0, tf_ecef_enu0);
-
-                    br_->sendTransform(tf_ecef_enu);
-                    br_->sendTransform(tf_ecef_poi);
-                    static_br_->sendTransform(tf_ecef_enu0);
+                    if (data.vrtk.fusion_status > 0) {
+                        geometry_msgs::msg::TransformStamped tf_ecef_poi;
+                        geometry_msgs::msg::TransformStamped tf_ecef_enu;
+                        geometry_msgs::msg::TransformStamped tf_ecef_enu0;
+                        TfDataToMsg(data.tf_ecef_poi, tf_ecef_poi);
+                        TfDataToMsg(data.tf_ecef_enu, tf_ecef_enu);
+                        TfDataToMsg(data.tf_ecef_enu0, tf_ecef_enu0);
+
+                        br_->sendTransform(tf_ecef_enu);
+                        br_->sendTransform(tf_ecef_poi);
+                        static_br_->sendTransform(tf_ecef_enu0);
+                    }
                 });
         } else if (format == "LLH" && converters_["LLH"]) {
             dynamic_cast<LlhConverter*>(converters_["LLH"].get())->AddObserver([this](const NavSatFixData& data) {