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 +94 -30
4 changed files with 94 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 8f4f7dba8b - Show all commits

6
toid_behaviors/src/move_coords.cpp
View File

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

114
toid_bt/behavior_trees/calibration_routines.xml
View File

@@ -1,40 +1,95 @@
<?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">
<RotateSimple angle="0"
max_speed="0.000000"
min_angle="270"
action_name=""/>
</Delay>
<SetBlackboard value="0.264"
output_key="width"/>
<Sleep msec="1000"/>
<ZeroOdom service_name=""/>
<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"
max_speed="0.050000"
action_name=""/>
<RotateSimple angle="0"
max_speed="0.000000"
min_angle="-270"
action_name=""/>
<ForceSuccess>
<Timeout msec="9000">
<MovePointSimple x="-0.2"
y="0"
theta="0"
max_speed="0.05"
backwards="true"
action_name=""/>
</Timeout>
</ForceSuccess>
</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">
<input_port name="x"
<Action ID="SetWidth">
<input_port name="width"
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>
<input_port name="count"
default="1"
type="int"/>
<output_port name="new_width"
type="double"/>
<input_port name="service_name"
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>

1
toid_bt/behavior_trees/toid_behaviors.btproj
View File

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

3
toid_bt/include/toid_bt/plugins/move_coords_action.hpp
View File

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