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

13
toid_bt/behavior_trees/behavior1.xml
View File

@@ -25,6 +25,8 @@
x="1.325" x="1.325"
topic_name="__default__placeholder__"/> topic_name="__default__placeholder__"/>
<WaitPullPin action_name=""/> <WaitPullPin action_name=""/>
<Timeout msec="100000">
<Sequence>
<InPose timeout="1.000000"> <InPose timeout="1.000000">
<MovePointSimple x="1.325" <MovePointSimple x="1.325"
y="0.200" y="0.200"
@@ -50,6 +52,8 @@
backwards="true" backwards="true"
action_name=""/> action_name=""/>
</Sequence> </Sequence>
</Timeout>
</Sequence>
</BehaviorTree> </BehaviorTree>
<BehaviorTree ID="seq2"> <BehaviorTree ID="seq2">
@@ -61,6 +65,8 @@
x="1.328" x="1.328"
topic_name="__default__placeholder__"/> topic_name="__default__placeholder__"/>
<WaitPullPin action_name=""/> <WaitPullPin action_name=""/>
<Timeout msec="100000">
<Sequence>
<InPose timeout="1.000000"> <InPose timeout="1.000000">
<MovePointSimple x="1.328" <MovePointSimple x="1.328"
y="0.354" y="0.354"
@@ -91,6 +97,7 @@
action_name=""/> action_name=""/>
<RotateTowards x="1.328" <RotateTowards x="1.328"
y="0.354" y="0.354"
backwards="true"
max_speed="2.000000" max_speed="2.000000"
min_angle="0.000000" min_angle="0.000000"
action_name=""/> action_name=""/>
@@ -127,6 +134,8 @@
</Sequence> </Sequence>
</Parallel> </Parallel>
</Sequence> </Sequence>
</Timeout>
</Sequence>
</BehaviorTree> </BehaviorTree>
<BehaviorTree ID="test_1"> <BehaviorTree ID="test_1">
@@ -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>

7
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);
if(!backwards) {
goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); 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;