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

1
toid_bt/behavior_trees/toid_behaviors.btproj
View File

@@ -44,6 +44,7 @@
<Action ID="RotateTowards"> <Action ID="RotateTowards">
<input_port name="x" type="double"/> <input_port name="x" type="double"/>
<input_port name="y" 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="max_speed" default="0.000000" type="double"/>
<input_port name="min_angle" 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> <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({ return providedBasicPorts({
BT::InputPort<double>("x", {}), BT::InputPort<double>("x", {}),
BT::InputPort<double>("y", {}), BT::InputPort<double>("y", {}),
BT::InputPort<bool>("backwards", false, {}),
BT::InputPort<double>("min_angle", 0, {}), BT::InputPort<double>("min_angle", 0, {}),
BT::InputPort<double>("max_speed", 0, {}), BT::InputPort<double>("max_speed", 0, {}),
//BT::InputPort<double>("options"), //BT::InputPort<double>("options"),
@@ -37,11 +38,17 @@ public:
auto y = getInput<double>("y").value(); auto y = getInput<double>("y").value();
auto min_angle = getInput<double>("min_angle").value(); auto min_angle = getInput<double>("min_angle").value();
auto max_speed = getInput<double>("max_speed").value(); auto max_speed = getInput<double>("max_speed").value();
auto backwards = getInput<bool>("backwards").value();
geometry_msgs::msg::PoseStamped pose; geometry_msgs::msg::PoseStamped pose;
get_pose(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.min_angle = angles::from_degrees(min_angle);
goal.max_speed = max_speed; goal.max_speed = max_speed;
goal.mode = 1; goal.mode = 1;