toid_bt/behavior_trees/behavior1.xml

@@ -25,6 +25,8 @@
                       x="1.325"
                       topic_name="__default__placeholder__"/>
       <WaitPullPin action_name=""/>
+      <Timeout msec="100000">
+        <Sequence>
           <InPose timeout="1.000000">
             <MovePointSimple x="1.325"
                              y="0.200"
@@ -50,6 +52,8 @@
                            backwards="true"
                            action_name=""/>
         </Sequence>
+      </Timeout>
+    </Sequence>
   </BehaviorTree>
 
   <BehaviorTree ID="seq2">
@@ -61,6 +65,8 @@
                       x="1.328"
                       topic_name="__default__placeholder__"/>
       <WaitPullPin action_name=""/>
+      <Timeout msec="100000">
+        <Sequence>
           <InPose timeout="1.000000">
             <MovePointSimple x="1.328"
                              y="0.354"
@@ -91,6 +97,7 @@
                           action_name=""/>
               <RotateTowards x="1.328"
                              y="0.354"
+                             backwards="true"
                              max_speed="2.000000"
                              min_angle="0.000000"
                              action_name=""/>
@@ -127,6 +134,8 @@
             </Sequence>
           </Parallel>
         </Sequence>
+      </Timeout>
+    </Sequence>
   </BehaviorTree>
 
   <BehaviorTree ID="test_1">
@@ -134,6 +143,7 @@
       <DetectStuck timeout="1.000000">
         <RotateTowards x="0.4"
                        y="0.0"
+                       backwards="false"
                        max_speed="0.000000"
                        min_angle="0.000000"
                        action_name=""/>
@@ -174,6 +184,9 @@
                   type="double"/>
       <input_port name="y"
                   type="double"/>
+      <input_port name="backwards"
+                  default="false"
+                  type="bool"/>
       <input_port name="max_speed"
                   default="0.000000"
                   type="double"/>

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -44,6 +44,7 @@
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
+            <input_port name="backwards" default="false" type="bool"/>
             <input_port name="max_speed" default="0.000000" type="double"/>
             <input_port name="min_angle" default="0.000000" type="double"/>
             <input_port name="action_name" type="std::string">Action server name</input_port>

toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp

@@ -25,6 +25,7 @@ public:
     return providedBasicPorts({
       BT::InputPort<double>("x", {}), 
       BT::InputPort<double>("y", {}), 
+      BT::InputPort<bool>("backwards", false, {}),
       BT::InputPort<double>("min_angle", 0, {}),
       BT::InputPort<double>("max_speed", 0, {}),
       //BT::InputPort<double>("options"),
@@ -37,11 +38,17 @@ public:
     auto y = getInput<double>("y").value();
     auto min_angle = getInput<double>("min_angle").value();
     auto max_speed = getInput<double>("max_speed").value();
+    auto backwards = getInput<bool>("backwards").value();
 
     geometry_msgs::msg::PoseStamped pose;
     get_pose(pose);
 
+    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;