diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml index e3023e1..b6becc0 100644 --- a/toid_bt/behavior_trees/behavior1.xml +++ b/toid_bt/behavior_trees/behavior1.xml @@ -3,31 +3,15 @@ - - - - - - Action server name - - diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml index 2a8d556..2f04d27 100644 --- a/toid_bt/behavior_trees/calibration_routines.xml +++ b/toid_bt/behavior_trees/calibration_routines.xml @@ -3,20 +3,38 @@ - + - - + + + + + + + - @@ -29,24 +47,32 @@ type="double"/> + Action server name - + Action server name - - Service name + + + + Action server name diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj index e8854a1..65d9521 100644 --- a/toid_bt/behavior_trees/toid_behaviors.btproj +++ b/toid_bt/behavior_trees/toid_behaviors.btproj @@ -11,18 +11,19 @@ + Action server name - + Action server name - + Action server name @@ -32,6 +33,11 @@ Service name + + + + Action server name + Service name diff --git a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp index 35127fc..bfb4fb2 100644 --- a/toid_bt/include/toid_bt/plugins/end_calib_action.hpp +++ b/toid_bt/include/toid_bt/plugins/end_calib_action.hpp @@ -23,7 +23,8 @@ public: static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("width"), BT::InputPort("count", 1, {}), + BT::InputPort("width"), + BT::InputPort("count", 1, {}), BT::OutputPort("new_width"), //BT::InputPort("options"), }); diff --git a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp index 6bc5b6c..4403516 100644 --- a/toid_bt/include/toid_bt/plugins/move_coords_action.hpp +++ b/toid_bt/include/toid_bt/plugins/move_coords_action.hpp @@ -24,7 +24,10 @@ public: static BT::PortsList providedPorts() { return providedBasicPorts({ - BT::InputPort("x"), BT::InputPort("y"), BT::InputPort("theta"), + BT::InputPort("x"), + BT::InputPort("y"), + BT::InputPort("theta"), + BT::InputPort("max_speed", 0, {}), //BT::InputPort("options"), }); } @@ -34,6 +37,8 @@ public: auto x_goal = getInput("x"); auto y_goal = getInput("y"); auto theta = getInput("theta"); + auto max_speed = getInput("max_speed").value(); + goal.x = x_goal.value(); goal.y = y_goal.value(); if (!theta.has_value()) { @@ -43,6 +48,9 @@ public: } else { goal.theta = angles::from_degrees(theta.value_or(0)); } + + goal.max_speed = max_speed; + return true; } diff --git a/toid_bt/include/toid_bt/plugins/rotate_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_action.hpp index be76e20..14ef835 100644 --- a/toid_bt/include/toid_bt/plugins/rotate_action.hpp +++ b/toid_bt/include/toid_bt/plugins/rotate_action.hpp @@ -25,7 +25,7 @@ public: return providedBasicPorts({ BT::InputPort("angle", {}), BT::InputPort("min_angle", 0, {}), - BT::InputPort("radians", false, {}), + BT::InputPort("max_speed", 0, {}), //BT::InputPort("options"), }); } @@ -34,13 +34,12 @@ public: { auto angle = getInput("angle"); auto min_angle = getInput("min_angle"); - auto radians = getInput("radians"); + auto max_speed = getInput("max_speed"); goal.angle = angle.value(); goal.min_angle = min_angle.value(); - if (!radians.value()) { - goal.angle = angles::from_degrees(goal.angle); - goal.min_angle = angles::from_degrees(goal.min_angle); - } + goal.max_speed = max_speed.value(); + goal.angle = angles::from_degrees(goal.angle); + goal.min_angle = angles::from_degrees(goal.min_angle); return true; } diff --git a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp index fb39f24..b2733f9 100644 --- a/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp +++ b/toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp @@ -26,7 +26,7 @@ public: BT::InputPort("x", {}), BT::InputPort("y", {}), BT::InputPort("min_angle", 0, {}), - BT::InputPort("radians", false, {}), + BT::InputPort("max_speed", 0, {}), //BT::InputPort("options"), }); } @@ -35,18 +35,15 @@ public: { auto x = getInput("x").value(); auto y = getInput("y").value(); - auto min_angle = getInput("min_angle"); - auto radians = getInput("radians"); + auto min_angle = getInput("min_angle").value(); + auto max_speed = getInput("max_speed").value(); geometry_msgs::msg::PoseStamped pose; get_pose(pose); goal.angle = std::atan2(y - pose.pose.position.y, x - pose.pose.position.x); - goal.min_angle = min_angle.value(); - - if (!radians.value()) { - goal.min_angle = angles::from_degrees(goal.min_angle); - } + goal.min_angle = angles::from_degrees(min_angle); + goal.max_speed = max_speed; return true; } diff --git a/toid_bt/include/toid_bt/plugins/translate_x_action.hpp b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp new file mode 100644 index 0000000..8ec9f6c --- /dev/null +++ b/toid_bt/include/toid_bt/plugins/translate_x_action.hpp @@ -0,0 +1,59 @@ +#pragma once + +#include "angles/angles.h" +#include "behaviortree_ros2/bt_action_node.hpp" +#include "tf2/utils.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "toid_bt/plugin.hpp" +#include "toid_msgs/action/simple_translate_x.hpp" + +namespace toid +{ + +class TranslateXNode : public BT::RosActionNode +{ +public: + TranslateXNode( + const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params, + PoseFunc pose) + : BT::RosActionNode(name, conf, params), get_pose(pose) + { + } + + static BT::PortsList providedPorts() + { + return providedBasicPorts({ + BT::InputPort("x"), + BT::InputPort("max_speed", 0, {}), + //BT::InputPort("options"), + }); + } + + bool setGoal(Goal & goal) override + { + auto x_goal = getInput("x").value(); + auto max_speed = getInput("max_speed").value(); + + goal.distance = x_goal; + goal.max_speed = max_speed; + + return true; + } + + void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); } + + BT::NodeStatus onResultReceived(const WrappedResult & wr) override + { + return (wr.result->error_code == 0) ? 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; + } + + PoseFunc get_pose; +}; + +} // namespace toid \ No newline at end of file diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp index 8679486..f4ef107 100644 --- a/toid_bt/src/bt_executor.cpp +++ b/toid_bt/src/bt_executor.cpp @@ -9,6 +9,7 @@ #include "toid_bt/plugins/move_coords_action.hpp" #include "toid_bt/plugins/rotate_action.hpp" #include "toid_bt/plugins/rotate_towards_action.hpp" +#include "toid_bt/plugins/translate_x_action.hpp" namespace toid { @@ -49,17 +50,17 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory) factory.registerNodeType( "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose); + factory.registerNodeType( + "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose); + factory.registerNodeType( "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose); - factory.registerNodeType( - "ZeroOdom", BT::RosNodeParams(nh, "/zero")); + factory.registerNodeType("ZeroOdom", BT::RosNodeParams(nh, "/zero")); - factory.registerNodeType( - "EndCalib", BT::RosNodeParams(nh, "/end_calib")); + factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib")); std::cout << describeCustomNodes() << std::endl; - } void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose)