Pimpest/toid
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

wip-behaviors #3

Merged
Pimpest merged 86 commits from wip-behaviors into main 2026-05-28 06:21:26 +00:00
Conversation 0 Commits 86 Files Changed 129 +107 -52
7 changed files with 107 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 980598718d - Show all commits

6
toid_behaviors/src/approach_acorns.cpp
View File

@@ -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.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);

3
toid_behaviors/src/rotate_acorns.cpp
View File

@@ -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) {

4
toid_bot_description/src/toid_bot_description.urdf
View File

@@ -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"/>

54
toid_bt/behavior_trees/behavior1.xml
View File

@@ -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"
y="0.18"
theta="-90"
max_speed="0.250000"
backwards="true"
<RotateTowards x="0.76"
y="-0.5"
max_speed="1.50000"
min_angle="0.000000"
action_name=""/>
<TranslateX x="0.5"
max_speed="0.300000"
<MovePointSimple x="0.756"
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>

29
toid_bt/behavior_trees/calibration_routines.xml
View File

@@ -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"

3
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -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"/>

10
toid_vision/src/toid_vision.cpp
View File

@@ -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_;