toid_bt/behavior_trees/toid_behaviors.btproj

@@ -4,59 +4,81 @@
     <include path="calibration_routines.xml"/>
     <!-- Description of Node Models (used by Groot) -->
     <TreeNodesModel>
+        <Action ID="ApproachAcorns">
+            <input_port name="x" type="double"/>
+            <input_port name="y" type="double"/>
+            <input_port name="theta" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="backwards" type="bool" default="false"/>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+        </Action>
         <Decorator ID="DetectStuck">
-            <input_port name="timeout" default="1.000000" type="double"/>
+            <input_port name="timeout" type="double" default="1.000000"/>
         </Decorator>
         <Action ID="EndCalib">
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
         <Decorator ID="InPose">
-            <input_port name="timeout" default="1.000000" type="double"/>
+            <input_port name="timeout" type="double" default="1.000000"/>
         </Decorator>
         <Action ID="MovePointSimple">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
             <input_port name="theta" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="backwards" default="false" type="bool"/>
+            <input_port name="backwards" type="bool" default="false"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+        </Action>
+        <Action ID="RotateAcorns">
+            <input_port name="angle" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
+            <input_port name="min_angle" type="double" default="0.000000"/>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="RotateSimple">
             <input_port name="angle" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="min_angle" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="min_angle" default="0.000000" type="double"/>
+            <input_port name="min_angle" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="Seq1">
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
         <Action ID="Seq2">
             <input_port name="text" type="std::string"/>
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
         <Action ID="Seq3">
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
+        </Action>
+        <Action ID="SetParameter">
+            <input_port name="parameter" type="std::string"/>
+            <input_port name="value" type="std::string"/>
+            <input_port name="node" type="std::string"/>
         </Action>
         <Action ID="SetWidth">
             <input_port name="width" type="double"/>
-            <input_port name="count" default="1" type="int"/>
+            <input_port name="count" type="int" default="1"/>
             <output_port name="new_width" type="double"/>
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
         <Action ID="TranslateX">
             <input_port name="x" type="double"/>
-            <input_port name="max_speed" default="0.000000" type="double"/>
+            <input_port name="max_speed" type="double" default="0.000000"/>
-            <input_port name="action_name" type="std::string">Action server name</input_port>
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
+        </Action>
+        <Action ID="WaitPullPin">
+            <input_port name="action_name" type="std::string" default="">Action server name</input_port>
         </Action>
         <Action ID="ZeroOdom">
-            <input_port name="service_name" type="std::string">Service name</input_port>
+            <input_port name="service_name" type="std::string" default="">Service name</input_port>
         </Action>
     </TreeNodesModel>
 </root>

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<toid_msgs::action::EmptyAction>
+{
+public:
+  PullPinNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosActionNode<toid_msgs::action::EmptyAction>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      //BT::InputPort<double>("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

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<rcl_interfaces::srv::SetParameters>
+{
+public:
+  SetParameterNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+  : BT::RosServiceNode<rcl_interfaces::srv::SetParameters>(name, conf, params)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<std::string>("node"),
+      BT::InputPort<std::string>("parameter"),
+      BT::InputPort<std::string>("value")
+    });
+  }
+
+  bool setRequest(typename Request::SharedPtr & request) override { 
+    std::string parameter = getInput<std::string>("parameter").value();
+    std::string value = getInput<std::string>("value").value();
+    std::string node = getInput<std::string>("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

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<toid::MovePointNode>(
     "MovePointSimple", BT::RosNodeParams(nh, "/moveCoords"), get_pose);
 
+  factory.registerNodeType<toid::MovePointNode>(
+    "ApproachAcorns", BT::RosNodeParams(nh, "/approachAcorns"), get_pose);
+
   factory.registerNodeType<toid::RotateNode>(
     "RotateSimple", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
+  factory.registerNodeType<toid::RotateNode>(
+    "RotateAcorns", BT::RosNodeParams(nh, "/rotateAcorns"), get_pose);
+
   factory.registerNodeType<toid::RotateTowardsNode>(
     "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
@@ -71,6 +79,11 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
   factory.registerNodeType<toid::EndCalibNode>(
     "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
 
+  factory.registerNodeType<toid::PullPinNode>("WaitPullPin", BT::RosNodeParams(nh, "/start_plug"));
+
+  factory.registerNodeType<toid::SetParameterNode>(
+    "SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
+
   factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
 
   factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));

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()