diff --git a/mg_bt/behavior_trees/mg_bt.btproj b/mg_bt/behavior_trees/mg_bt.btproj
index 7863dc6..53cee8e 100644
--- a/mg_bt/behavior_trees/mg_bt.btproj
+++ b/mg_bt/behavior_trees/mg_bt.btproj
@@ -6,65 +6,86 @@
-
+
- Service name
+ Service name
+
+
+
+
+
+
-
-
+
+
- Action server name
+ Action server name
-
- Action server name
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+ Action server name
+
+
+
+
+
+
+
+ Action server name
+
+
+
+
+
+
+
+
+
+
+ Action server name
+
+
+
+
+
+
+
+
+
+ Action server name
-
-
-
-
-
-
-
-
-
-
-
- Action server name
+
+
+
+ Action server name
-
- Service name
+
+ Service name
- Topic name
+ Topic name
-
+
- Service name
+ Service name
diff --git a/mg_bt/behavior_trees/tactics.xml b/mg_bt/behavior_trees/tactics.xml
index a77c040..445df99 100644
--- a/mg_bt/behavior_trees/tactics.xml
+++ b/mg_bt/behavior_trees/tactics.xml
@@ -1,5 +1,260 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
Action server name
-
+
+
+
+
+ Action server name
+
+
+
+
+
+
+
+
+
+ Action server name
+
+
+
+
+
+
+
+
+
+
+ Action server name
+
+
+
+
+
+
+
+
+
+ Action server name
+
+
-
- Service name
+
+
+
+ Action server name
+ type="bool"/>
("MovePoint", node());
+ factory.registerNodeType("MoveGlobal", node());
factory.registerNodeType("RotateNode", node());
factory.registerNodeType("ZeroNode", node());
factory.registerNodeType("I2CSignal", node());
factory.registerNodeType("SideObstaclePub", node());
factory.registerNodeType("SendPose", node());
+ factory.registerNodeType("LookAtNode", node());
+ factory.registerNodeType("MoveLocal", node(), [this]() { return this->position(); });
+ factory.registerNodeType("CanChooser");
factory.registerNodeType("TacticChooser");
factory.registerNodeType("CalibWidth", node(), [this]() { return this->position(); });
}
diff --git a/mg_bt/src/tree_nodes/choose_can.hpp b/mg_bt/src/tree_nodes/choose_can.hpp
new file mode 100644
index 0000000..6776ebc
--- /dev/null
+++ b/mg_bt/src/tree_nodes/choose_can.hpp
@@ -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("canlist"),
+ BT::OutputPort("x_loc"),
+ BT::OutputPort("y_loc"),
+ BT::OutputPort("orient") };
+ }
+
+ // You must override the virtual function tick()
+ BT::NodeStatus tick() override {
+ constexpr std::array 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("canlist").value());
+ ss >> idx;
+
+ setOutput("x_loc", c[idx].x);
+ setOutput("y_loc", c[idx].y);
+ setOutput("orient", c[idx].orientation);
+
+ return BT::NodeStatus::SUCCESS;
+ }
+ };
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/look_at.hpp b/mg_bt/src/tree_nodes/look_at.hpp
new file mode 100644
index 0000000..f435ace
--- /dev/null
+++ b/mg_bt/src/tree_nodes/look_at.hpp
@@ -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 {
+ public:
+ LookAtNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+ BT::RosActionNode(name, conf, params) { }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({ BT::InputPort("x"),
+ BT::InputPort("y"),
+ BT::InputPort("tolerance", 0.001, {}),
+ BT::InputPort("max_wheel_speed", 3, {}),
+ BT::InputPort("max_angular", 3.14, {})
+
+ });
+ }
+
+ bool setGoal(Goal& goal) override {
+ auto x = getInput("x");
+ auto y = getInput("y");
+ auto tolerance = getInput("tolerance");
+ auto max_wheel_speed = getInput("max_wheel_speed");
+ auto max_angular = getInput("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;
+ }
+ };
+
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/move_global.hpp b/mg_bt/src/tree_nodes/move_global.hpp
new file mode 100644
index 0000000..a9d5441
--- /dev/null
+++ b/mg_bt/src/tree_nodes/move_global.hpp
@@ -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 {
+ public:
+ MoveGlobalNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
+ BT::RosActionNode(name, conf, params) { }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({ BT::InputPort("x_goal"),
+ BT::InputPort("y_goal"),
+ BT::InputPort("tolerance", 0.05, {}),
+ BT::InputPort("max_wheel_speed", 6.0, {}),
+ BT::InputPort("max_angular", 3.14, {}),
+ BT::InputPort("max_vel", 4.0, {}),
+ BT::InputPort("ornt_mult", 3.0, {}) });
+ }
+
+ bool setGoal(Goal& goal) override {
+ auto x_goal = getInput("x_goal");
+ auto y_goal = getInput("y_goal");
+ auto tolerance = getInput("tolerance");
+ auto max_wheel_speed = getInput("max_wheel_speed");
+ auto max_angular = getInput("max_angular");
+ auto max_vel = getInput("max_vel");
+ auto pos_mult = getInput("pos_mult");
+ auto ornt_mult = getInput("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;
+ }
+ };
+
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/move_local.hpp b/mg_bt/src/tree_nodes/move_local.hpp
new file mode 100644
index 0000000..31a686e
--- /dev/null
+++ b/mg_bt/src/tree_nodes/move_local.hpp
@@ -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 {
+ using PosFuncSig = std::function()>;
+
+ PosFuncSig pos_query_;
+
+ public:
+ MoveLocalNode(const std::string& name,
+ const BT::NodeConfig& conf,
+ const BT::RosNodeParams& params,
+ const PosFuncSig pos_query) :
+ BT::RosActionNode(name, conf, params), pos_query_(pos_query) { }
+
+ static BT::PortsList providedPorts() {
+ return providedBasicPorts({ BT::InputPort("x_goal"),
+ BT::InputPort("y_goal"),
+ BT::InputPort("tolerance", 0.001, {}),
+ BT::InputPort("max_wheel_speed", 3, {}),
+ BT::InputPort("max_angular", 3.14, {}),
+ BT::InputPort("max_vel", 2, {}),
+ BT::InputPort("pos_mult", 15, {}),
+ BT::InputPort("ornt_mult", 4, {}) });
+ }
+
+ bool setGoal(Goal& goal) override {
+ auto x_goal = getInput("x_goal");
+ auto y_goal = getInput("y_goal");
+ auto tolerance = getInput("tolerance");
+ auto max_wheel_speed = getInput("max_wheel_speed");
+ auto max_angular = getInput("max_angular");
+ auto max_vel = getInput("max_vel");
+ auto pos_mult = getInput("pos_mult");
+ auto ornt_mult = getInput("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;
+ }
+ };
+
+}
\ No newline at end of file
diff --git a/mg_bt/src/tree_nodes/move_point.hpp b/mg_bt/src/tree_nodes/move_point.hpp
index 2bf0cc4..89af108 100644
--- a/mg_bt/src/tree_nodes/move_point.hpp
+++ b/mg_bt/src/tree_nodes/move_point.hpp
@@ -13,17 +13,11 @@ namespace mg {
static BT::PortsList providedPorts() {
return providedBasicPorts({ BT::InputPort("x_goal"),
BT::InputPort("y_goal"),
- BT::InputPort("tolerance"),
- BT::InputPort("max_wheel_speed"),
- BT::InputPort("max_angular"),
- BT::InputPort("max_vel"),
- BT::InputPort("pos_mult"),
- BT::InputPort("ornt_mult"),
- BT::InputPort("t_ornt_mult"),
- BT::InputPort("obst_mult_a"),
- BT::InputPort("obst_mult_b"),
- BT::InputPort("obst_mult_c"),
- BT::InputPort("IgnoreList", "", {}) });
+ BT::InputPort("tolerance", 0.001, {}),
+ BT::InputPort("max_wheel_speed", 3, {}),
+ BT::InputPort("max_angular", 3.14, {}),
+ BT::InputPort("max_vel", 2, {}),
+ BT::InputPort("ornt_mult", 4, {}) });
}
bool setGoal(Goal& goal) override {
@@ -35,10 +29,6 @@ namespace mg {
auto max_vel = getInput("max_vel");
auto pos_mult = getInput("pos_mult");
auto ornt_mult = getInput("ornt_mult");
- auto t_ornt_mult = getInput("t_ornt_mult");
- auto obst_mult_a = getInput("obst_mult_a");
- auto obst_mult_b = getInput("obst_mult_b");
- auto obst_mult_c = getInput("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;
}
diff --git a/mg_bt/src/tree_nodes/rotate.hpp b/mg_bt/src/tree_nodes/rotate.hpp
index 377b844..859eeba 100644
--- a/mg_bt/src/tree_nodes/rotate.hpp
+++ b/mg_bt/src/tree_nodes/rotate.hpp
@@ -11,18 +11,12 @@ namespace mg {
BT::RosActionNode(name, conf, params) { }
static BT::PortsList providedPorts() {
- return providedBasicPorts({ BT::InputPort("angle"),
- BT::InputPort("tolerance"),
- BT::InputPort("max_wheel_speed"),
- BT::InputPort("max_angular"),
- BT::InputPort("max_vel"),
- BT::InputPort("pos_mult"),
- BT::InputPort("ornt_mult"),
- BT::InputPort("t_ornt_mult"),
- BT::InputPort("obst_mult_a"),
- BT::InputPort("obst_mult_b"),
- BT::InputPort("obst_mult_c"),
- BT::InputPort("IgnoreList", "", {}) });
+ return providedBasicPorts({
+ BT::InputPort("angle"),
+ BT::InputPort("tolerance", 0.001, {}),
+ BT::InputPort("max_wheel_speed", 3, {}),
+ BT::InputPort("max_angular", 3.14, {}),
+ });
}
bool setGoal(Goal& goal) override {
@@ -30,24 +24,10 @@ namespace mg {
auto tolerance = getInput("tolerance");
auto max_wheel_speed = getInput("max_wheel_speed");
auto max_angular = getInput("max_angular");
- auto max_vel = getInput("max_vel");
- auto pos_mult = getInput("pos_mult");
- auto ornt_mult = getInput("ornt_mult");
- auto t_ornt_mult = getInput("t_ornt_mult");
- auto obst_mult_a = getInput("obst_mult_a");
- auto obst_mult_b = getInput("obst_mult_b");
- auto obst_mult_c = getInput("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;
}
diff --git a/mg_navigation/include/handlers/dwa_core.hpp b/mg_navigation/include/handlers/dwa_core.hpp
index 81edbe2..5f03aeb 100644
--- a/mg_navigation/include/handlers/dwa_core.hpp
+++ b/mg_navigation/include/handlers/dwa_core.hpp
@@ -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
- cvl = spacevl[b_ind];
- cvr = spacevr[b_ind];
+ 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();
}
diff --git a/mg_navigation/include/handlers/dwa_point.hpp b/mg_navigation/include/handlers/dwa_point.hpp
index 996337f..94b009e 100644
--- a/mg_navigation/include/handlers/dwa_point.hpp
+++ b/mg_navigation/include/handlers/dwa_point.hpp
@@ -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) }));
diff --git a/mg_planner/src/mg_planner_node.cpp b/mg_planner/src/mg_planner_node.cpp
index c9e95df..0c5f1f2 100644
--- a/mg_planner/src/mg_planner_node.cpp
+++ b/mg_planner/src/mg_planner_node.cpp
@@ -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 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(resp->indicies);
path_pub_->publish(path);
- RCLCPP_INFO(get_logger(), "Handled request in %lf seconds", (get_clock()->now() - elapsed).seconds());
});
path_pub_ = create_publisher("/GeneratedPath", rclcpp::QoS(4).keep_last(2));