From 980598718d60d28952d3cbb6065045496db6b1f4 Mon Sep 17 00:00:00 2001 From: Pimpest <82343504+pimpest@users.noreply.github.com> Date: Wed, 15 Apr 2026 15:32:28 +0200 Subject: [PATCH] various fixes --- toid_behaviors/src/approach_acorns.cpp | 6 +- toid_behaviors/src/rotate_acorns.cpp | 3 +- .../src/toid_bot_description.urdf | 4 +- toid_bt/behavior_trees/behavior1.xml | 56 +++++++++----- .../behavior_trees/calibration_routines.xml | 77 ++++++++++++------- toid_bt/behavior_trees/toid_behaviors.btproj | 3 + toid_vision/src/toid_vision.cpp | 10 ++- 7 files changed, 107 insertions(+), 52 deletions(-) diff --git a/toid_behaviors/src/approach_acorns.cpp b/toid_behaviors/src/approach_acorns.cpp index 1a8bad1..8965f1b 100644 --- a/toid_behaviors/src/approach_acorns.cpp +++ b/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( - "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.y += std::sin(yaw) * -0.35 + std::cos(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.010; tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation); yaw = angles::normalize_angle(yaw); diff --git a/toid_behaviors/src/rotate_acorns.cpp b/toid_behaviors/src/rotate_acorns.cpp index c6292b5..405ca0a 100644 --- a/toid_behaviors/src/rotate_acorns.cpp +++ b/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( - "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) { diff --git a/toid_bot_description/src/toid_bot_description.urdf b/toid_bot_description/src/toid_bot_description.urdf index c9ce36a..55efc49 100644 --- a/toid_bot_description/src/toid_bot_description.urdf +++ b/toid_bot_description/src/toid_bot_description.urdf @@ -11,7 +11,7 @@ - + @@ -22,7 +22,7 @@ - + diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index e2a1bdb..11880ca 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -2,6 +2,9 @@ + + + @@ -12,21 +15,40 @@ - - + + + + - @@ -49,6 +71,11 @@ default="1.000000" type="double"/> + + + @@ -93,15 +120,6 @@ Service name - - - - Action server name - Service name diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index 19b8c42..d7c4d79 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -22,7 +22,7 @@ @@ -36,7 +36,7 @@ @@ -47,30 +47,55 @@ - - - - - - - - + + + + + + + + + + + + + + + + + Service name + + + diff --git a/toid_vision/src/toid_vision.cpp b/toid_vision/src/toid_vision.cpp index 4b0dadc..369df71 100644 --- a/toid_vision/src/toid_vision.cpp +++ b/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("/closest_acorn", rclcpp::QoS(5)); + this->create_publisher("/closest_acorn", rclcpp::QoS(1).keep_last(1)); flip_pub_ = this->create_publisher("/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_;