diff --git a/toid_bt/behavior_trees/toid_behaviors.btproj b/toid_bt/behavior_trees/toid_behaviors.btproj
index 1a2994b..adf08cc 100644
--- a/toid_bt/behavior_trees/toid_behaviors.btproj
+++ b/toid_bt/behavior_trees/toid_behaviors.btproj
@@ -4,59 +4,81 @@
+
+
+
+
+
+
+ Action server name
+
-
+
- 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
- Service name
+ Service name
- Service name
+ Service name
- Service name
+ Service name
+
+
+
+
+
-
+
- Service name
+ Service name
-
- Action server name
+
+ Action server name
+
+
+ Action server name
- Service name
+ Service name
diff --git a/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp b/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp
new file mode 100644
index 0000000..71561a2
--- /dev/null
+++ b/toid_bt/include/toid_bt/plugins/pull_pin_action.hpp
@@ -0,0 +1,48 @@
+#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/empty_action.hpp"
+
+namespace toid
+{
+
+class PullPinNode : public BT::RosActionNode
+{
+public:
+ PullPinNode(
+ 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("options"),
+ });
+ }
+
+ bool setGoal(Goal &) override
+ {
+ return true;
+ }
+
+ void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
+
+ BT::NodeStatus onResultReceived(const WrappedResult &) override
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+
+ BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
+ {
+ RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
+ return BT::NodeStatus::FAILURE;
+ }
+};
+
+} // namespace toid
\ No newline at end of file
diff --git a/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp b/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp
new file mode 100644
index 0000000..9d973ba
--- /dev/null
+++ b/toid_bt/include/toid_bt/plugins/set_parameter_action.hpp
@@ -0,0 +1,50 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_service_node.hpp"
+#include "std_srvs/srv/empty.hpp"
+#include "tf2/utils.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "toid_bt/plugin.hpp"
+#include "rcl_interfaces/srv/set_parameters.hpp"
+
+namespace toid
+{
+
+class SetParameterNode : public BT::RosServiceNode
+{
+public:
+ SetParameterNode(
+ const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+ : BT::RosServiceNode(name, conf, params)
+ {
+ }
+
+ static BT::PortsList providedPorts()
+ {
+ return providedBasicPorts({
+ BT::InputPort("node"),
+ BT::InputPort("parameter"),
+ BT::InputPort("value")
+ });
+ }
+
+ bool setRequest(typename Request::SharedPtr & request) override {
+ std::string parameter = getInput("parameter").value();
+ std::string value = getInput("value").value();
+ std::string node = getInput("node").value();
+
+ setServiceName("/" + node + "/set_parameters");
+ request->parameters.emplace_back();
+ request->parameters.front().name = parameter;
+ request->parameters.front().value.string_value = value;
+ return true;
+}
+
+ BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
+ {
+ return BT::NodeStatus::SUCCESS;
+ }
+};
+
+} // namespace
\ No newline at end of file
diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp
index 124e6ef..93e7874 100644
--- a/toid_bt/src/bt_executor.cpp
+++ b/toid_bt/src/bt_executor.cpp
@@ -8,9 +8,11 @@
#include "toid_bt/plugins/end_calib_action.hpp"
#include "toid_bt/plugins/in_position_decorator.hpp"
#include "toid_bt/plugins/move_coords_action.hpp"
+#include "toid_bt/plugins/pull_pin_action.hpp"
#include "toid_bt/plugins/rotate_action.hpp"
#include "toid_bt/plugins/rotate_towards_action.hpp"
#include "toid_bt/plugins/send_text_action.hpp"
+#include "toid_bt/plugins/set_parameter_action.hpp"
#include "toid_bt/plugins/stuck_detector_decorator.hpp"
#include "toid_bt/plugins/translate_x_action.hpp"
@@ -59,9 +61,15 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType(
"MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
+ factory.registerNodeType(
+ "ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose);
+
factory.registerNodeType(
"RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);
+ factory.registerNodeType(
+ "RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose);
+
factory.registerNodeType(
"RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
@@ -71,6 +79,11 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType(
"SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
+ factory.registerNodeType("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));
+
+ factory.registerNodeType(
+ "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
+
factory.registerNodeType("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
diff --git a/toid_interaction/toid_interaction/interaction_node.py b/toid_interaction/toid_interaction/interaction_node.py
index 842736e..f3c225d 100644
--- a/toid_interaction/toid_interaction/interaction_node.py
+++ b/toid_interaction/toid_interaction/interaction_node.py
@@ -107,7 +107,9 @@ def main(args=None):
rclpy.init(args=args)
node = InteracitionNode()
- rclpy.executors.MultiThreadedExecutor().spin(node)
+ executor = rclpy.executors.MultiThreadedExecutor()
+ executor.add_node(node)
+ executor.spin()
rclpy.shutdown()