toid_behaviors/src/move_coords.cpp

@@ -95,10 +95,6 @@ ResultStatus MoveCoords::updateVel(
   const double current_yaw = tf2::getYaw(pose.orientation);
   double angle_dist = angles::shortest_angular_distance(current_yaw, target_angle_);
 
-  if (backwards_) {
-    angle_dist = angles::two_pi_complement(angle_dist);
-  }
-
   const double dx = target_pose_.position.x - pose.position.x;
   const double dy = target_pose_.position.y - pose.position.y;
 
@@ -120,7 +116,7 @@ ResultStatus MoveCoords::updateVel(
 
   if (dist_left <= 0.02) {
     out_vel.linear.x = std::clamp(target_sign_ * vel, lower_bound, upper_bound);
-    out_vel.angular.z = std::clamp(target_sign_ * kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
+    out_vel.angular.z = std::clamp(kp_ * angle_dist, -max_angular_speed_, max_angular_speed_);
     last_speed_ = out_vel.linear.x;
     return ResultStatus{Status::RUNNING};
   }

toid_bt/behavior_trees/calibration_routines.xml

@@ -1,39 +1,94 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <root BTCPP_format="4">
-  <BehaviorTree ID="wheel_ratio_calibration">
+  <BehaviorTree ID="track_calib">
     <Sequence>
-      <Delay delay_msec="5000">
+      <SetBlackboard value="0.264"
-        <RotateSimple angle="0"
+                     output_key="width"/>
-                      max_speed="0.000000"
+      <Sleep msec="1000"/>
-                      min_angle="270"
+      <ZeroOdom service_name=""/>
-                      action_name=""/>
-      </Delay>
       <Sequence>
-        <MovePointSimple x="1.0"
+        <Sleep msec="1000"/>
+        <MovePointSimple x="-0.4"
                          y="0"
                          theta="0"
                          max_speed="0.10000"
+                         backwards="true"
                          action_name=""/>
-        <RotateSimple angle="180"
+        <Sleep msec="500"/>
+        <RotateSimple angle="0"
                       max_speed="0.500000"
+                      min_angle="270"
+                      action_name=""/>
+      </Sequence>
+      <ForceSuccess>
+        <Timeout msec="13000">
+          <MovePointSimple x="1.0"
+                           y="0"
+                           theta="0"
+                           max_speed="0.070000"
+                           backwards="false"
+                           action_name=""/>
+        </Timeout>
+      </ForceSuccess>
+      <SetWidth width="{width}"
+                count="1"
+                new_width="{width}"
+                service_name=""/>
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="wheel_ratio_calibration">
+    <Sequence>
+      <Sleep msec="5000"/>
+      <ZeroOdom service_name=""/>
+      <Sleep msec="1000"/>
+      <Sequence>
+        <MovePointSimple x="1.1"
+                         y="0"
+                         theta="0"
+                         max_speed="0.10000"
+                         backwards="false"
+                         action_name=""/>
+        <Sleep msec="1000"/>
+        <RotateSimple angle="180"
+                      max_speed="0.300000"
                       min_angle="10"
                       action_name=""/>
-        <MovePointSimple x="0.4"
+        <Sleep msec="500"/>
+        <MovePointSimple x="0.35"
                          y="0"
                          theta="180"
                          max_speed="0.100000"
+                         backwards="false"
                          action_name=""/>
         <RotateSimple angle="0"
-                      max_speed="0.500000"
+                      max_speed="0.300000"
                       min_angle="-10"
                       action_name=""/>
       </Sequence>
-      <TranslateX x="-0.5"
+      <ForceSuccess>
-                  max_speed="0.050000"
+        <Timeout msec="9000">
+          <MovePointSimple x="-0.2"
+                           y="0"
+                           theta="0"
+                           max_speed="0.05"
+                           backwards="true"
                            action_name=""/>
-      <RotateSimple angle="0"
+        </Timeout>
-                    max_speed="0.000000"
+      </ForceSuccess>
-                    min_angle="-270"
+    </Sequence>
+  </BehaviorTree>
+
+  <BehaviorTree ID="wheel_size">
+    <Sequence>
+      <Sleep msec="1000"/>
+      <ZeroOdom service_name=""/>
+      <Sleep msec="1000"/>
+      <MovePointSimple x="-1.200"
+                       y="0"
+                       theta="0"
+                       max_speed="0.250000"
+                       backwards="true"
                        action_name=""/>
     </Sequence>
   </BehaviorTree>
@@ -50,6 +105,9 @@
       <input_port name="max_speed"
                   default="0.000000"
                   type="double"/>
+      <input_port name="backwards"
+                  default="false"
+                  type="bool"/>
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
@@ -65,14 +123,20 @@
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
-    <Action ID="TranslateX">
+    <Action ID="SetWidth">
-      <input_port name="x"
+      <input_port name="width"
                   type="double"/>
-      <input_port name="max_speed"
+      <input_port name="count"
-                  default="0.000000"
+                  default="1"
+                  type="int"/>
+      <output_port name="new_width"
                    type="double"/>
-      <input_port name="action_name"
+      <input_port name="service_name"
-                  type="std::string">Action server name</input_port>
+                  type="std::string">Service name</input_port>
+    </Action>
+    <Action ID="ZeroOdom">
+      <input_port name="service_name"
+                  type="std::string">Service name</input_port>
     </Action>
   </TreeNodesModel>
 

toid_bt/behavior_trees/toid_behaviors.btproj

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

toid_bt/include/toid_bt/plugins/move_coords_action.hpp

@@ -28,6 +28,7 @@ public:
       BT::InputPort<double>("y"),
       BT::InputPort<double>("theta"),
       BT::InputPort<double>("max_speed", 0, {}),
+      BT::InputPort<bool>("backwards", false, {}),
       //BT::InputPort<double>("options"),
     });
   }
@@ -38,6 +39,7 @@ public:
     auto y_goal = getInput<double>("y");
     auto theta = getInput<double>("theta");
     auto max_speed = getInput<double>("max_speed").value();
+    auto backwards = getInput<bool>("backwards").value();
 
     goal.x = x_goal.value();
     goal.y = y_goal.value();
@@ -50,6 +52,7 @@ public:
     }
 
     goal.max_speed = max_speed;
+    goal.backwards = backwards;
 
     return true;
   }