toid_behaviors/src/approach_acorns.cpp

@@ -68,7 +68,7 @@ void ApproachAcorns::activateCB()
   auto node = node_.lock();
   using namespace std::placeholders;
   acorn_pose_sub_ = node->create_subscription<PoseStamped>(
-    "closest_acorn", rclcpp::QoS(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1));
+    "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&ApproachAcorns::acorn_position_cb, this, _1));
   target_pose_pub_->on_activate();
   distance_ = 1.0;
 }
@@ -110,8 +110,8 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
 
   yaw += M_PI / 2;
 
-  pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * -0.005;
+  pose_global.pose.position.x += std::cos(yaw) * -0.35 - std::sin(yaw) * 0.010;
-  pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * -0.005;
+  pose_global.pose.position.y += std::sin(yaw) * -0.35 + std::cos(yaw) * 0.010;
   tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation);
 
   yaw = angles::normalize_angle(yaw);

toid_behaviors/src/rotate_acorns.cpp

@@ -43,7 +43,7 @@ void RotateAcorns::activateCB()
   auto node = node_.lock();
   using namespace std::placeholders;
   acorn_pose_sub_ = node->create_subscription<PoseStamped>(
-    "closest_acorn", rclcpp::QoS(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
+    "closest_acorn", rclcpp::QoS(2).keep_last(2), std::bind(&RotateAcorns::acorn_position_cb, this, _1));
   distance_ = 1.0;
 }
 
@@ -101,6 +101,7 @@ ResultStatus RotateAcorns::onStart(
   initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
   max_angular_speed_ = command->max_speed;
   mode_ = command->mode;
+  distance_ = 1.0;
   avg_.reset();
 
   if (command->max_speed == 0) {

toid_bot_description/src/toid_bot_description.urdf

@@ -11,7 +11,7 @@
 <xacro:property name="wheel_width" value="0.03"/>
 
 <xacro:property name="wheel_zoff" value="0.03"/>
-<xacro:property name="wheel_xoff" value="-0.07"/>
+<xacro:property name="wheel_xoff" value="-0.105"/>
 <xacro:property name="wheel_inset" value="0.01"/>
 
 <xacro:property name="caster_radius" value="0.02"/>
@@ -22,7 +22,7 @@
 
 <xacro:property name="lidar_radius" value="0.03"/>
 <xacro:property name="lidar_height" value="0.02"/>
-<xacro:property name="lidar_xoff" value="0.09"/>
+<xacro:property name="lidar_xoff" value="0.00"/>
 
 <xacro:property name="camera_xoff" value="0.07369"/>
 <xacro:property name="camera_zoff" value="0.16664"/>

toid_bt/behavior_trees/behavior1.xml

@@ -2,6 +2,9 @@
 <root BTCPP_format="4">
   <BehaviorTree ID="seq">
     <Sequence>
+      <InPose timeout="1.000000">
+        <Sleep msec="999999"/>
+      </InPose>
       <Seq1 service_name=""/>
       <Seq2 text="0101"
             service_name=""/>
@@ -12,20 +15,39 @@
   <BehaviorTree ID="seq1">
     <Sequence>
       <ZeroOdom service_name=""/>
-      <MovePointSimple x="1.05"
+      <MovePointSimple x="0.1"
                        y="0"
                        theta="0"
-                       max_speed="0.300000"
+                       max_speed="0.350000"
                        backwards="false"
                        action_name=""/>
-      <MovePointSimple x="0.76"
+      <RotateTowards x="0.76"
-                       y="0.18"
+                     y="-0.5"
-                       theta="-90"
+                     max_speed="1.50000"
-                       max_speed="0.250000"
+                     min_angle="0.000000"
-                       backwards="true"
                      action_name=""/>
-      <TranslateX x="0.5"
+      <MovePointSimple x="0.756"
-                  max_speed="0.300000"
+                       y="-0.5"
+                       theta="-45"
+                       max_speed="0.350000"
+                       backwards="false"
+                       action_name=""/>
+      <RotateTowards x="0.76"
+                     y="-0.02"
+                     max_speed="1.50000"
+                     min_angle="0.000000"
+                     action_name=""/>
+      <MovePointSimple x="0.757"
+                       y="-0.02"
+                       theta="90"
+                       max_speed="0.150000"
+                       backwards="false"
+                       action_name=""/>
+      <MovePointSimple x="1.2"
+                       y="0.0"
+                       theta="0.0"
+                       max_speed="0.250000"
+                       backwards="false"
                        action_name=""/>
     </Sequence>
   </BehaviorTree>
@@ -49,6 +71,11 @@
                   default="1.000000"
                   type="double"/>
     </Decorator>
+    <Decorator ID="InPose">
+      <input_port name="timeout"
+                  default="1.000000"
+                  type="double"/>
+    </Decorator>
     <Action ID="MovePointSimple">
       <input_port name="x"
                   type="double"/>
@@ -93,15 +120,6 @@
       <input_port name="service_name"
                   type="std::string">Service name</input_port>
     </Action>
-    <Action ID="TranslateX">
-      <input_port name="x"
-                  type="double"/>
-      <input_port name="max_speed"
-                  default="0.000000"
-                  type="double"/>
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-    </Action>
     <Action ID="ZeroOdom">
       <input_port name="service_name"
                   type="std::string">Service name</input_port>

toid_bt/behavior_trees/calibration_routines.xml

@@ -22,7 +22,7 @@
         <Sleep msec="500"/>
         <RotateSimple angle="0"
                       max_speed="0.500000"
-                      min_angle="270"
+                      min_angle="700"
                       action_name=""/>
       </Sequence>
       <ForceSuccess>
@@ -36,7 +36,7 @@
         </DetectStuck>
       </ForceSuccess>
       <SetWidth width="{width}"
-                count="1"
+                count="2"
                 new_width="{width}"
                 service_name=""/>
     </Sequence>
@@ -47,6 +47,7 @@
       <Sleep msec="5000"/>
       <ZeroOdom service_name=""/>
       <Sleep msec="1000"/>
+      <Repeat num_cycles="6">
         <Sequence>
           <MovePointSimple x="1.1"
                            y="0"
@@ -70,7 +71,31 @@
                         max_speed="0.300000"
                         min_angle="-10"
                         action_name=""/>
+          <Sleep msec="500"/>
+          <MovePointSimple x="1.1"
+                           y="0"
+                           theta="0"
+                           max_speed="0.10000"
+                           backwards="false"
+                           action_name=""/>
+          <Sleep msec="500"/>
+          <RotateSimple angle="180"
+                        max_speed="0.300000"
+                        min_angle="-10"
+                        action_name=""/>
+          <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.300000"
+                        min_angle="10"
+                        action_name=""/>
         </Sequence>
+      </Repeat>
       <ForceSuccess>
         <DetectStuck timeout="1.000000">
           <MovePointSimple x="-0.2"

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -10,6 +10,9 @@
         <Action ID="EndCalib">
             <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>
+        <Decorator ID="InPose">
+            <input_port name="timeout" default="1.000000" type="double"/>
+        </Decorator>
         <Action ID="MovePointSimple">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>

toid_vision/src/toid_vision.cpp

@@ -37,7 +37,7 @@ NutDetector::NutDetector(const rclcpp::NodeOptions & options)
   RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ");
 
   pose_pub_ =
-    this->create_publisher<geometry_msgs::msg::PoseStamped>("/closest_acorn", rclcpp::QoS(5));
+    this->create_publisher<geometry_msgs::msg::PoseStamped>("/closest_acorn", rclcpp::QoS(1).keep_last(1));
   flip_pub_ = this->create_publisher<std_msgs::msg::String>("/to_flip", rclcpp::QoS(1));
 }
 
@@ -80,12 +80,16 @@ void NutDetector::process_image(
 
   size_t prev = 0;
   std::string colors = "";
+  size_t first_ok = -1;
 
   for (size_t i = 0; i < markerIds_.size(); i++) {
     const int idx = markers_[i].second;
     if (markerIds_[idx] != 47 && markerIds_[idx] != 36) {
       continue;
     }
+    if (first_ok == (size_t)-1) {
+      first_ok = idx;
+    }
 
     RCLCPP_DEBUG(
       this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x,
@@ -131,6 +135,10 @@ void NutDetector::process_image(
     flip_pub_->publish(m);
   }
 
+  if (first_ok == (size_t)-1) {
+    return;
+  }
+
   geometry_msgs::msg::PoseStamped pose;
   get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose);
   pose.header.frame_id = camera_frame_id_;