From 49bbd58e49d35a48d846fb2332a084408f0db6cd Mon Sep 17 00:00:00 2001
From: Pimpest <82343504+pimpest@users.noreply.github.com>
Date: Fri, 17 Apr 2026 14:03:32 +0200
Subject: [PATCH] Rotate towards backwards
---
toid_bt/behavior_trees/behavior1.xml | 175 ++++++++++--------
toid_bt/behavior_trees/toid_behaviors.btproj | 1 +
.../toid_bt/plugins/rotate_towards_action.hpp | 9 +-
3 files changed, 103 insertions(+), 82 deletions(-)
diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml
index d9b071a..0c0d1b5 100644
--- a/toid_bt/behavior_trees/behavior1.xml
+++ b/toid_bt/behavior_trees/behavior1.xml
@@ -25,30 +25,34 @@
x="1.325"
topic_name="__default__placeholder__"/>
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
@@ -61,71 +65,76 @@
x="1.328"
topic_name="__default__placeholder__"/>
-
-
-
-
-
-
-
-
-
-
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
+
@@ -134,6 +143,7 @@
@@ -174,6 +184,9 @@
type="double"/>
+
diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj
index 78b09b6..655d646 100644
--- a/toid_bt/behavior_trees/toid_behaviors.btproj
+++ b/toid_bt/behavior_trees/toid_behaviors.btproj
@@ -44,6 +44,7 @@
+
Action server name
diff --git a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp
index 9649a8f..4cf1382 100644
--- a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp
+++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp
@@ -25,6 +25,7 @@ public:
return providedBasicPorts({
BT::InputPort("x", {}),
BT::InputPort("y", {}),
+ BT::InputPort("backwards", false, {}),
BT::InputPort("min_angle", 0, {}),
BT::InputPort("max_speed", 0, {}),
//BT::InputPort("options"),
@@ -37,11 +38,17 @@ public:
auto y = getInput("y").value();
auto min_angle = getInput("min_angle").value();
auto max_speed = getInput("max_speed").value();
+ auto backwards = getInput("backwards").value();
geometry_msgs::msg::PoseStamped pose;
get_pose(pose);
- goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
+ if(!backwards) {
+ goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
+ }
+ if(backwards) {
+ goal.angle = std::atan2(pose.pose.position.y - y, pose.pose.position.x - x);
+ }
goal.min_angle = angles::from_degrees(min_angle);
goal.max_speed = max_speed;
goal.mode = 1;