Fix problem of atan2

OUXT-Polaris · Oct 29, 2024 · 7ca20de · 7ca20de
1 parent bad12df
commit 7ca20de
Showing 1 changed file with 7 additions and 0 deletions.
diff --git a/robotx_behavior_tree/include/robotx_behavior_tree/action_node.hpp b/robotx_behavior_tree/include/robotx_behavior_tree/action_node.hpp
@@ -315,6 +315,13 @@ class ActionROS2Node : public BT::StatefulActionNode, public rclcpp::Node
     }
     double delta_x = obj.x - current_pose.value()->pose.position.x;
     double delta_y = obj.y - current_pose.value()->pose.position.y;
+    const double minimum_delta = 0.1;
+    if (abs(delta_x) < minimum_delta) {
+      delta_x = minimum_delta;
+    }
+    if (abs(delta_y) < minimum_delta) {
+      delta_y = minimum_delta;
+    }
     double theta = std::atan2(delta_y, delta_x);
     geometry_msgs::msg::Pose p;
     p.position.x = obj.x - distance * std::cos(theta);