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

91
mg_bt/behavior_trees/mg_bt.btproj
View File

@ -6,65 +6,86 @@
<TreeNodesModel>
<Action ID="CalibWidth">
<input_port name="PreviousWidth" type="double"/>
<input_port name="Count" type="int" default="1"/>
<input_port name="Count" default="1" type="int"/>
<output_port name="NewWidth" type="double"/>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Action ID="CanChooser">
<output_port name="orient" type="int"/>
<output_port name="y_loc" type="double"/>
<output_port name="x_loc" type="double"/>
<input_port name="canlist" type="std::string"/>
</Action>
<Action ID="I2CSignal">
<input_port name="Address" type="int" default="42"/>
<input_port name="Data" type="int" default="0"/>
<input_port name="Address" default="42" type="int"/>
<input_port name="Data" default="0" type="int"/>
<output_port name="Result" type="int"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="MovePoint">
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="tolerance" type="double"/>
<input_port name="obst_mult_a" type="double"/>
<input_port name="ornt_mult" type="double"/>
<input_port name="pos_mult" type="double"/>
<input_port name="t_ornt_mult" type="double"/>
<input_port name="obst_mult_c" type="double"/>
<input_port name="obst_mult_b" type="double"/>
<input_port name="max_vel" type="double"/>
<input_port name="max_angular" type="double"/>
<input_port name="IgnoreList" type="std::string" default=""/>
<input_port name="max_wheel_speed" type="double"/>
<Action ID="LookAtNode">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveGlobal">
<input_port name="x_goal" type="double"/>
<input_port name="y_goal" type="double"/>
<input_port name="max_wheel_speed" default="6.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="max_vel" default="4.000000" type="double"/>
<input_port name="ornt_mult" default="3.000000" type="double"/>
<input_port name="tolerance" default="0.050000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveLocal">
<input_port name="x_goal" type="double"/>
<input_port name="y_goal" type="double"/>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="max_vel" default="2.000000" type="double"/>
<input_port name="pos_mult" default="15.000000" type="double"/>
<input_port name="ornt_mult" default="4.000000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="MovePoint">
<input_port name="x_goal" type="double"/>
<input_port name="y_goal" type="double"/>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="max_vel" default="2.000000" type="double"/>
<input_port name="ornt_mult" default="4.000000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateNode">
<input_port name="angle" type="double"/>
<input_port name="pos_mult" type="double"/>
<input_port name="max_wheel_speed" type="double"/>
<input_port name="IgnoreList" type="std::string" default=""/>
<input_port name="max_angular" type="double"/>
<input_port name="max_vel" type="double"/>
<input_port name="obst_mult_b" type="double"/>
<input_port name="obst_mult_c" type="double"/>
<input_port name="ornt_mult" type="double"/>
<input_port name="t_ornt_mult" type="double"/>
<input_port name="obst_mult_a" type="double"/>
<input_port name="tolerance" type="double"/>
<input_port name="action_name" type="std::string" default="">Action server name</input_port>
<input_port name="max_wheel_speed" default="3.000000" type="double"/>
<input_port name="max_angular" default="3.140000" type="double"/>
<input_port name="tolerance" default="0.001000" type="double"/>
<input_port name="action_name" type="std::string">Action server name</input_port>
</Action>
<Action ID="SendPose">
<input_port name="x" type="double"/>
<input_port name="y" type="double"/>
<input_port name="angle" type="double"/>
<input_port name="isDegree" type="bool" default="true"/>
<input_port name="service_name" type="std::string" default="">Service name</input_port>
<input_port name="isDegree" default="true" type="bool"/>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic" type="int"/>
<input_port name="topic_name" type="std::string" default="__default__placeholder__">Topic name</input_port>
<input_port name="topic_name" default="__default__placeholder__" type="std::string">Topic name</input_port>
</Condition>
<Action ID="TacticChooser">
<output_port name="IsBlue" type="int"/>
<output_port name="IsBlue" type="bool"/>
<output_port name="tactic" type="int"/>
<input_port name="number" type="int"/>
</Action>
<Action ID="ZeroNode">
<input_port name="service_name" type="std::string" default="">Service name</input_port>
<input_port name="service_name" type="std::string">Service name</input_port>
</Action>
</TreeNodesModel>
</root>

406
mg_bt/behavior_trees/tactics.xml
View File

@ -1,5 +1,260 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="pickup_can">
<Sequence>
<CanChooser orient="{can_orient}"
y_loc="{y_can}"
x_loc="{x_can}"
canlist="{target_can}"/>
<MoveGlobal x_goal="{x_can}"
y_goal="{y_can}"
max_wheel_speed="6.000000"
max_angular="3.140000"
max_vel="4.000000"
ornt_mult="3.000000"
tolerance="0.050000"
action_name="/MoveGlobal"/>
<LookAtNode x="{x_can}"
y="{y_can}"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/LookAt"/>
<MovePoint x_goal="{x_can}"
y_goal="{y_can}"
max_wheel_speed="5.000000"
max_angular="0.1"
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<Sequence>
<SubTree ID="pickup_right"
_skipIf="can_orient!=0"/>
<SubTree ID="pickup_down"
_skipIf="can_orient!=1"/>
<SubTree ID="pickup_left"
_skipIf="can_orient!=2"/>
<SubTree ID="pickup_up"
_skipIf="can_orient!=3"
_autoremap="false"/>
<MovePoint x_goal="0.8"
y_goal="0.6"
max_wheel_speed="3.000000"
max_angular="3.140000"
max_vel="2.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"
_skipIf="target_can!=1"/>
</Sequence>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_down">
<Sequence>
<RotateNode angle="-1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="-0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0"
y_goal="0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="-0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0"
y_goal="0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_left">
<Sequence>
<RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="-0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="-3.14159265359"
max_wheel_speed="5.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="-0.12"
y_goal="0.0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0.12"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_right">
<Sequence>
<RotateNode angle="-3.14159265359"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="-0.11"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="0"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0.12"
y_goal="0.0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="-0.12"
y_goal="0"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="pickup_up">
<Sequence>
<RotateNode angle="1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0"
y_goal="-0.11"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<RotateNode angle="-1.57079632679"
max_wheel_speed="3.000000"
max_angular="3.140000"
tolerance="0.01000"
action_name="/Rotate"/>
<MoveLocal x_goal="0"
y_goal="0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
<MoveLocal x_goal="0.0"
y_goal="-0.12"
max_wheel_speed="5.000000"
max_angular="3.140000"
max_vel="2.000000"
pos_mult="15.000000"
ornt_mult="4.000000"
tolerance="0.001000"
action_name="/MovePoint"/>
</Sequence>
</BehaviorTree>
<BehaviorTree ID="tactic_base">
<Sequence>
<I2CSignal Address="66"
@ -29,23 +284,45 @@
<BehaviorTree ID="testbed">
<Sequence>
<SendPose x="1.0"
y="1.0"
angle="90"
isDegree="true"
service_name="/set_pose"/>
<Delay delay_msec="5000">
<SendPose x="2.0"
y="0.5"
angle="-1"
isDegree="false"
service_name="/set_pose"/>
</Delay>
<SubTree ID="pickup_can"
target_can="0"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="1"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="2"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="3"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="6"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="8"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="5"
_autoremap="false"/>
<SubTree ID="pickup_can"
target_can="4"
_autoremap="false"/>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Action ID="CanChooser">
<output_port name="orient"
type="int"/>
<output_port name="y_loc"
type="double"/>
<output_port name="x_loc"
type="double"/>
<input_port name="canlist"
type="std::string"/>
</Action>
<Action ID="I2CSignal">
<input_port name="Address"
default="42"
@ -58,18 +335,109 @@
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="SendPose">
<Action ID="LookAtNode">
<input_port name="x"
type="double"/>
<input_port name="y"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveGlobal">
<input_port name="x_goal"
type="double"/>
<input_port name="y_goal"
type="double"/>
<input_port name="max_wheel_speed"
default="6.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="max_vel"
default="4.000000"
type="double"/>
<input_port name="ornt_mult"
default="3.000000"
type="double"/>
<input_port name="tolerance"
default="0.050000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="MoveLocal">
<input_port name="x_goal"
type="double"/>
<input_port name="y_goal"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="max_vel"
default="2.000000"
type="double"/>
<input_port name="pos_mult"
default="15.000000"
type="double"/>
<input_port name="ornt_mult"
default="4.000000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="MovePoint">
<input_port name="x_goal"
type="double"/>
<input_port name="y_goal"
type="double"/>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="max_vel"
default="2.000000"
type="double"/>
<input_port name="ornt_mult"
default="4.000000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Action ID="RotateNode">
<input_port name="angle"
type="double"/>
<input_port name="isDegree"
default="true"
type="bool"/>
<input_port name="service_name"
type="std::string">Service name</input_port>
<input_port name="max_wheel_speed"
default="3.000000"
type="double"/>
<input_port name="max_angular"
default="3.140000"
type="double"/>
<input_port name="tolerance"
default="0.001000"
type="double"/>
<input_port name="action_name"
type="std::string">Action server name</input_port>
</Action>
<Condition ID="SideObstaclePub">
<input_port name="tactic"
@ -80,7 +448,7 @@
</Condition>
<Action ID="TacticChooser">
<output_port name="IsBlue"
type="int"/>
type="bool"/>
<output_port name="tactic"
type="int"/>
<input_port name="number"

5
mg_bt/launch/launch.py
View File

@ -18,5 +18,10 @@ def generate_launch_description():
output="screen",
parameters=[bt_exec_config]
),
Node(
package='mg_planner',
executable='mg_planner',
output="screen"
),
])

8
mg_bt/src/mg_tree_executor.cpp
View File

@ -2,9 +2,13 @@
#include "behaviortree_cpp/xml_parsing.h"
#include "tree_nodes/calib.hpp"
#include "tree_nodes/choose_can.hpp"
#include "tree_nodes/choose_tactic.hpp"
#include "tree_nodes/i2c.hpp"
#include "tree_nodes/move_point.hpp"
#include "tree_nodes/move_local.hpp"
#include "tree_nodes/move_global.hpp"
#include "tree_nodes/look_at.hpp"
#include "tree_nodes/rotate.hpp"
#include "tree_nodes/side_pub.hpp"
#include "tree_nodes/set_pos.hpp"
@ -28,11 +32,15 @@ namespace mg {
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
factory.registerNodeType<mg::MoveGlobalNode>("MoveGlobal", node());
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
factory.registerNodeType<mg::I2cNode>("I2CSignal", node());
factory.registerNodeType<mg::SidePub>("SideObstaclePub", node());
factory.registerNodeType<mg::SendPoseNode>("SendPose", node());
factory.registerNodeType<mg::LookAtNode>("LookAtNode", node());
factory.registerNodeType<mg::MoveLocalNode>("MoveLocal", node(), [this]() { return this->position(); });
factory.registerNodeType<mg::CanChooser>("CanChooser");
factory.registerNodeType<mg::TacticsChooser>("TacticChooser");
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
}

49
mg_bt/src/tree_nodes/choose_can.hpp Normal file
View File

@ -0,0 +1,49 @@
#include "behaviortree_cpp/action_node.h"
namespace mg {
class CanChooser : public BT::SyncActionNode {
struct cans {
double x;
double y;
int orientation;
};
public:
CanChooser(const std::string& name, const BT::NodeConfig config) : BT::SyncActionNode(name, config) { }
static BT::PortsList providedPorts() {
// This action has a single input port called "message"
return { BT::InputPort<std::string>("canlist"),
BT::OutputPort<double>("x_loc"),
BT::OutputPort<double>("y_loc"),
BT::OutputPort<int>("orient") };
}
// You must override the virtual function tick()
BT::NodeStatus tick() override {
constexpr std::array<cans, 10> c{ { { 0.825, 1.425, 3 },
{ 0.35, 0.4, 2 },
{ 0.35, 1.325, 2 },
{ 0.775, 0.55, 1 },
{ 2.225, 0.55, 1 },
{ 2.65, 0.4, 0 },
{ 2.65, 0.4, 0 },
{ 2.175, 1.425, 3 },
{ 1.1, 0.65, 3 },
{ 1.9, 0.65, 3 } } };
int idx = 0;
// Todo iterate through all cans and use a passthrough function that returns current can state
std::istringstream ss(getInput<std::string>("canlist").value());
ss >> idx;
setOutput<double>("x_loc", c[idx].x);
setOutput<double>("y_loc", c[idx].y);
setOutput<int>("orient", c[idx].orientation);
return BT::NodeStatus::SUCCESS;
}
};
}

50
mg_bt/src/tree_nodes/look_at.hpp Normal file
View File

@ -0,0 +1,50 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/look_at.hpp"
namespace mg {
class LookAtNode : public BT::RosActionNode<mg_msgs::action::LookAt> {
public:
LookAtNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosActionNode<mg_msgs::action::LookAt>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x"),
BT::InputPort<double>("y"),
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 {
auto x = getInput<double>("x");
auto y = getInput<double>("y");
auto tolerance = getInput<double>("tolerance");
auto max_wheel_speed = getInput<double>("max_wheel_speed");
auto max_angular = getInput<double>("max_angular");
goal.x = x.value();
goal.y = y.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);
return true;
}
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
BT::NodeStatus::FAILURE;
}
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
return BT::NodeStatus::FAILURE;
}
};
}

58
mg_bt/src/tree_nodes/move_global.hpp Normal file
View File

@ -0,0 +1,58 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/move_global.hpp"
namespace mg {
class MoveGlobalNode : public BT::RosActionNode<mg_msgs::action::MoveGlobal> {
public:
MoveGlobalNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
BT::RosActionNode<mg_msgs::action::MoveGlobal>(name, conf, params) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
BT::InputPort<double>("y_goal"),
BT::InputPort<double>("tolerance", 0.05, {}),
BT::InputPort<double>("max_wheel_speed", 6.0, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
BT::InputPort<double>("max_vel", 4.0, {}),
BT::InputPort<double>("ornt_mult", 3.0, {}) });
}
bool setGoal(Goal& goal) override {
auto x_goal = getInput<double>("x_goal");
auto y_goal = getInput<double>("y_goal");
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");
goal.x.push_back(x_goal.value());
goal.y.push_back(y_goal.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);
return true;
}
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
BT::NodeStatus::FAILURE;
}
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
return BT::NodeStatus::FAILURE;
}
};
}

67
mg_bt/src/tree_nodes/move_local.hpp Normal file
View File

@ -0,0 +1,67 @@
#pragma once
#include "behaviortree_ros2/bt_action_node.hpp"
#include "mg_msgs/action/move_point.hpp"
#include "glm/glm.hpp"
namespace mg {
class MoveLocalNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
using PosFuncSig = std::function<std::pair<glm::vec2, double>()>;
PosFuncSig pos_query_;
public:
MoveLocalNode(const std::string& name,
const BT::NodeConfig& conf,
const BT::RosNodeParams& params,
const PosFuncSig pos_query) :
BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params), pos_query_(pos_query) { }
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
BT::InputPort<double>("y_goal"),
BT::InputPort<double>("tolerance", 0.001, {}),
BT::InputPort<double>("max_wheel_speed", 3, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
BT::InputPort<double>("max_vel", 2, {}),
BT::InputPort<double>("pos_mult", 15, {}),
BT::InputPort<double>("ornt_mult", 4, {}) });
}
bool setGoal(Goal& goal) override {
auto x_goal = getInput<double>("x_goal");
auto y_goal = getInput<double>("y_goal");
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 [pos, theta] = pos_query_();
goal.x = x_goal.value() + pos.x;
goal.y = y_goal.value() + pos.y;
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);
return true;
}
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
return (wr.result->error == ActionType::Result::SUCCESS) ? BT::NodeStatus::SUCCESS :
BT::NodeStatus::FAILURE;
}
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override {
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
return BT::NodeStatus::FAILURE;
}
};
}

24
mg_bt/src/tree_nodes/move_point.hpp
View File

@ -13,17 +13,11 @@ namespace mg {
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
BT::InputPort<double>("y_goal"),
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", "", {}) });
BT::InputPort<double>("tolerance", 0.001, {}),
BT::InputPort<double>("max_wheel_speed", 3, {}),
BT::InputPort<double>("max_angular", 3.14, {}),
BT::InputPort<double>("max_vel", 2, {}),
BT::InputPort<double>("ornt_mult", 4, {}) });
}
bool setGoal(Goal& goal) override {
@ -35,10 +29,6 @@ namespace mg {
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.x = x_goal.value();
goal.y = y_goal.value();
goal.tolerance = tolerance.value_or(goal.tolerance);
@ -47,10 +37,6 @@ namespace mg {
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;
}

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

12
mg_navigation/include/handlers/dwa_core.hpp
View File

@ -125,11 +125,14 @@ namespace mg {
pos_query();
populate_candidates(spacevl, spacevr, dimx, dimy);
double b_score = calc_score(spacevl[0], spacevr[0]);
uint b_ind = 0;
double b_score = calc_score(spacevl[0], spacevr[0]) - 1;
int b_ind = -1;
for (uint i = 1; i < spacevl.size(); i++) {
const double score = calc_score(spacevl[i], spacevr[i]);
if (score == NAN) {
continue;
}
if (score > b_score) {
b_score = score;
b_ind = i;
@ -138,8 +141,13 @@ namespace mg {
calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
if (b_ind >= 0) {
cvl = spacevl[b_ind];
cvr = spacevr[b_ind];
} else {
cvl = 0;
cvr = 0;
}
send_speed(step * cvl, step * cvr);
rate.sleep();
}

7
mg_navigation/include/handlers/dwa_point.hpp
View File

@ -49,6 +49,13 @@ namespace mg {
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta);
const double dist = baseNode.obstacle_manager()->box_colide(
n_pos, { 0.29, 0.33 }, n_theta, ObstacleManager::ObstacleMask::DYNAMIC);
if (dist < 0.01) {
return NAN;
}
// Calculate angle to goal post/pre movement
const double angl = glm::asin(glm::determinant(glm::dmat2{ face, glm::normalize(target_pos - pos) }));
const double n_angl = glm::asin(glm::determinant(glm::dmat2{ n_face, glm::normalize(target_pos - n_pos) }));

2
mg_planner/src/mg_planner_node.cpp
View File

@ -17,7 +17,6 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
"GeneratePath",
[this](mg_msgs::srv::CalcPath::Request::ConstSharedPtr req,
mg_msgs::srv::CalcPath::Response::SharedPtr resp) -> void {
std::cout << "Bro recieved request\n";
auto elapsed = get_clock()->now();
std::vector<glm::ivec2> goals;
std::transform(req->x.begin(),
@ -38,7 +37,6 @@ mg::PlannerNode::PlannerNode() : Node("planner"), astar_(this) {
mg_msgs::msg::Path path;
path.indicies = std::vector<int>(resp->indicies);
path_pub_->publish(path);
RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
});
path_pub_ = create_publisher<mg_msgs::msg::Path>("/GeneratedPath", rclcpp::QoS(4).keep_last(2));