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

Rotate towards backwards

Browse Source
This commit is contained in:
Pimpest
2026-04-17 14:03:32 +02:00
parent b3ea552cfb
commit 49bbd58e49
3 changed files with 103 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files

175
toid_bt/behavior_trees/behavior1.xml
View File

@@ -25,30 +25,34 @@
x="1.325"
topic_name="__default__placeholder__"/>
<WaitPullPin action_name=""/>
<InPose timeout="1.000000">
<MovePointSimple x="1.325"
y="0.200"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
</InPose>
<Seq1 service_name=""/>
<Seq2 text=""
service_name=""/>
<MovePointSimple x="1.325"
y="-0.05"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
<Seq3 service_name=""/>
<MovePointSimple x="1.325"
y="0.805"
theta="-90"
max_speed="0.200000"
backwards="true"
action_name=""/>
<Timeout msec="100000">
<Sequence>
<InPose timeout="1.000000">
<MovePointSimple x="1.325"
y="0.200"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
</InPose>
<Seq1 service_name=""/>
<Seq2 text=""
service_name=""/>
<MovePointSimple x="1.325"
y="-0.05"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
<Seq3 service_name=""/>
<MovePointSimple x="1.325"
y="0.805"
theta="-90"
max_speed="0.200000"
backwards="true"
action_name=""/>
</Sequence>
</Timeout>
</Sequence>
</BehaviorTree>
@@ -61,71 +65,76 @@
x="1.328"
topic_name="__default__placeholder__"/>
<WaitPullPin action_name=""/>
<InPose timeout="1.000000">
<MovePointSimple x="1.328"
y="0.354"
theta="-90"
max_speed="0.300000"
backwards="false"
action_name=""/>
</InPose>
<Seq1 service_name=""/>
<Parallel failure_count="1"
success_count="-1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="0.795"
y="-0.2"
theta="180"
max_speed="0.500000"
backwards="false"
action_name=""/>
</Parallel>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Timeout msec="100000">
<Sequence>
<Sleep msec="500"/>
<TranslateX x="-0.3"
max_speed="0.000000"
action_name=""/>
<RotateTowards x="1.328"
y="0.354"
max_speed="2.000000"
min_angle="0.000000"
action_name=""/>
</Sequence>
</Parallel>
<MovePointSimple x="1.328"
y="0.354"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
<Seq1 service_name=""/>
<ParallelAll max_failures="1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="1.328"
y="-0.2"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
</ParallelAll>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Sequence>
<Sleep msec="500"/>
<InPose timeout="1.000000">
<MovePointSimple x="1.328"
y="0.354"
theta="-90"
max_speed="0.300000"
backwards="false"
action_name=""/>
</InPose>
<Seq1 service_name=""/>
<Parallel failure_count="1"
success_count="-1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="0.795"
y="-0.2"
theta="180"
max_speed="0.500000"
backwards="false"
action_name=""/>
</Parallel>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Sequence>
<Sleep msec="500"/>
<TranslateX x="-0.3"
max_speed="0.000000"
action_name=""/>
<RotateTowards x="1.328"
y="0.354"
backwards="true"
max_speed="2.000000"
min_angle="0.000000"
action_name=""/>
</Sequence>
</Parallel>
<MovePointSimple x="1.328"
y="0.354"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
<Seq1 service_name=""/>
<ParallelAll max_failures="1">
<Seq2 text=""
service_name=""/>
<MovePointSimple x="1.328"
y="-0.2"
theta="-90"
max_speed="0.200000"
backwards="false"
action_name=""/>
</ParallelAll>
<Parallel failure_count="1"
success_count="-1">
<Seq3 service_name=""/>
<Sequence>
<Sleep msec="500"/>
<MovePointSimple x="1.328"
y="0.354"
theta="-90"
max_speed="0.400000"
backwards="true"
action_name=""/>
</Sequence>
</Parallel>
</Sequence>
</Parallel>
</Timeout>
</Sequence>
</BehaviorTree>
@@ -134,6 +143,7 @@
<DetectStuck timeout="1.000000">
<RotateTowards x="0.4"
y="0.0"
backwards="false"
max_speed="0.000000"
min_angle="0.000000"
action_name=""/>
@@ -174,6 +184,9 @@
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="backwards"
default="false"
type="bool"/>
<input_port name="max_speed"
default="0.000000"
type="double"/>

1
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -44,6 +44,7 @@
<Action ID="RotateTowards">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="backwards" default="false" type="bool"/>
<input_port name="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" default="0.000000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>

9
toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp
View File

@@ -25,6 +25,7 @@ public:
return providedBasicPorts({
BT::InputPort<double>("x", {}),
BT::InputPort<double>("y", {}),
BT::InputPort<bool>("backwards", false, {}),
BT::InputPort<double>("min_angle", 0, {}),
BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"),
@@ -37,11 +38,17 @@ public:
auto y = getInput<double>("y").value();
auto min_angle = getInput<double>("min_angle").value();
auto max_speed = getInput<double>("max_speed").value();
auto backwards = getInput<bool>("backwards").value();
geometry_msgs::msg::PoseStamped pose;
get_pose(pose);
goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
if(!backwards) {
goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x);
}
if(backwards) {
goal.angle = std::atan2(pose.pose.position.y - y, pose.pose.position.x - x);
}
goal.min_angle = angles::from_degrees(min_angle);
goal.max_speed = max_speed;
goal.mode = 1;