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

various fixes

Browse Source
This commit is contained in:
Pimpest
2026-04-15 15:32:28 +02:00
parent 4d4c598ce9
commit 980598718d
7 changed files with 107 additions and 52 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
toid_behaviors/src/approach_acorns.cpp
View File

@@ -68,7 +68,7 @@ void ApproachAcorns::activateCB()
auto node = node_.lock(); auto node = node_.lock();
using namespace std::placeholders; using namespace std::placeholders;
acorn_pose_sub_ = node->create_subscription<PoseStamped>( 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(); target_pose_pub_->on_activate();
distance_ = 1.0; distance_ = 1.0;
} }
@@ -110,8 +110,8 @@ void ApproachAcorns::acorn_position_cb(PoseStamped::ConstSharedPtr msg)
yaw += M_PI / 2; 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); tf2::convert(tf2::Quaternion::createFromRPY(0, 0, yaw), pose_global.pose.orientation);
yaw = angles::normalize_angle(yaw); 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(); auto node = node_.lock();
using namespace std::placeholders; using namespace std::placeholders;
acorn_pose_sub_ = node->create_subscription<PoseStamped>( 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; distance_ = 1.0;
} }
@@ -101,6 +101,7 @@ ResultStatus RotateAcorns::onStart(
initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0; initial_direction_ = (command->min_angle >= 0.0) ? 1.0 : -1.0;
max_angular_speed_ = command->max_speed; max_angular_speed_ = command->max_speed;
mode_ = command->mode; mode_ = command->mode;
distance_ = 1.0;
avg_.reset(); avg_.reset();
if (command->max_speed == 0) { 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_width" value="0.03"/>
<xacro:property name="wheel_zoff" 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="wheel_inset" value="0.01"/>
<xacro:property name="caster_radius" value="0.02"/> <xacro:property name="caster_radius" value="0.02"/>
@@ -22,7 +22,7 @@
<xacro:property name="lidar_radius" value="0.03"/> <xacro:property name="lidar_radius" value="0.03"/>
<xacro:property name="lidar_height" value="0.02"/> <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_xoff" value="0.07369"/>
<xacro:property name="camera_zoff" value="0.16664"/> <xacro:property name="camera_zoff" value="0.16664"/>

56
toid_bt/behavior_trees/behavior1.xml
View File

@@ -2,6 +2,9 @@
<root BTCPP_format="4"> <root BTCPP_format="4">
<BehaviorTree ID="seq"> <BehaviorTree ID="seq">
<Sequence> <Sequence>
<InPose timeout="1.000000">
<Sleep msec="999999"/>
</InPose>
<Seq1 service_name=""/> <Seq1 service_name=""/>
<Seq2 text="0101" <Seq2 text="0101"
service_name=""/> service_name=""/>
@@ -12,21 +15,40 @@
<BehaviorTree ID="seq1"> <BehaviorTree ID="seq1">
<Sequence> <Sequence>
<ZeroOdom service_name=""/> <ZeroOdom service_name=""/>
<MovePointSimple x="1.05" <MovePointSimple x="0.1"
y="0" y="0"
theta="0" theta="0"
max_speed="0.300000" max_speed="0.350000"
backwards="false" backwards="false"
action_name=""/> 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=""/>
<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=""/> action_name=""/>
<TranslateX x="0.5"
max_speed="0.300000"
action_name=""/>
</Sequence> </Sequence>
</BehaviorTree> </BehaviorTree>
@@ -49,6 +71,11 @@
default="1.000000" default="1.000000"
type="double"/> type="double"/>
</Decorator> </Decorator>
<Decorator ID="InPose">
<input_port name="timeout"
default="1.000000"
type="double"/>
</Decorator>
<Action ID="MovePointSimple"> <Action ID="MovePointSimple">
<input_port name="x" <input_port name="x"
type="double"/> type="double"/>
@@ -93,15 +120,6 @@
<input_port name="service_name" <input_port name="service_name"
type="std::string">Service name</input_port> type="std::string">Service name</input_port>
</Action> </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"> <Action ID="ZeroOdom">
<input_port name="service_name" <input_port name="service_name"
type="std::string">Service name</input_port> type="std::string">Service name</input_port>

77
toid_bt/behavior_trees/calibration_routines.xml
View File

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

3
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -10,6 +10,9 @@
<Action ID="EndCalib"> <Action ID="EndCalib">
<input_port name="service_name" type="std::string">Service name</input_port> <input_port name="service_name" type="std::string">Service name</input_port>
</Action> </Action>
<Decorator ID="InPose">
<input_port name="timeout" default="1.000000" type="double"/>
</Decorator>
<Action ID="MovePointSimple"> <Action ID="MovePointSimple">
<input_port name="x" type="double"/> <input_port name="x" type="double"/>
<input_port name="y" 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"); RCLCPP_INFO(this->get_logger(), "NutDetector activated... scanning for DEEZ NUTZ");
pose_pub_ = 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)); 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; size_t prev = 0;
std::string colors = ""; std::string colors = "";
size_t first_ok = -1;
for (size_t i = 0; i < markerIds_.size(); i++) { for (size_t i = 0; i < markerIds_.size(); i++) {
const int idx = markers_[i].second; const int idx = markers_[i].second;
if (markerIds_[idx] != 47 && markerIds_[idx] != 36) { if (markerIds_[idx] != 47 && markerIds_[idx] != 36) {
continue; continue;
} }
if (first_ok == (size_t)-1) {
first_ok = idx;
}
RCLCPP_DEBUG( RCLCPP_DEBUG(
this->get_logger(), "Marker %ld at %lf %lf", i, markerCorners_[idx][0].x, 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); flip_pub_->publish(m);
} }
if (first_ok == (size_t)-1) {
return;
}
geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::PoseStamped pose;
get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose); get_pose(tvecs[markers_[0].second], rvecs[markers_[0].second], pose.pose);
pose.header.frame_id = camera_frame_id_; pose.header.frame_id = camera_frame_id_;