Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

First set of behaviors

Browse Source
This commit is contained in:
Pimpest
2025-05-08 12:10:13 +02:00
parent da3d1d9d15
commit bce371dba2
13 changed files with 710 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
mg_bt/src/tree_nodes/rotate.hpp
View File

@ -11,18 +11,12 @@ namespace mg {
BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("angle"),
BT::InputPort<double>("tolerance"),
BT::InputPort<double>("max_wheel_speed"),
BT::InputPort<double>("max_angular"),
BT::InputPort<double>("max_vel"),
BT::InputPort<double>("pos_mult"),
BT::InputPort<double>("ornt_mult"),
BT::InputPort<double>("t_ornt_mult"),
BT::InputPort<double>("obst_mult_a"),
BT::InputPort<double>("obst_mult_b"),
BT::InputPort<double>("obst_mult_c"),
BT::InputPort<std::string>("IgnoreList", "", {}) });
return providedBasicPorts({
BT::InputPort<double>("angle"),
BT::InputPort<double>("tolerance", 0.001, {}),
BT::InputPort<double>("max_wheel_speed", 3, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
});
}
bool setGoal(Goal& goal) override {
@ -30,24 +24,10 @@ namespace mg {
auto tolerance = getInput<double>("tolerance");
auto max_wheel_speed = getInput<double>("max_wheel_speed");
auto max_angular = getInput<double>("max_angular");
auto max_vel = getInput<double>("max_vel");
auto pos_mult = getInput<double>("pos_mult");
auto ornt_mult = getInput<double>("ornt_mult");
auto t_ornt_mult = getInput<double>("t_ornt_mult");
auto obst_mult_a = getInput<double>("obst_mult_a");
auto obst_mult_b = getInput<double>("obst_mult_b");
auto obst_mult_c = getInput<double>("obst_mult_c");
goal.angle = angle.value();
goal.tolerance = tolerance.value_or(goal.tolerance);
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
goal.max_angular = max_angular.value_or(goal.max_angular);
goal.max_vel = max_vel.value_or(goal.max_vel);
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
return true;
}