toid_bt/behavior_trees/behavior1.xml

@@ -3,31 +3,15 @@
   <BehaviorTree ID="test_1">
     <Sequence>
       <RotateSimple angle="90"
-                    radians="false"
                     min_angle="0.000000"
                     action_name=""/>
       <Sleep msec="1000"/>
       <RotateSimple angle="0"
-                    radians="false"
                     min_angle="0.000000"
                     action_name=""/>
     </Sequence>
   </BehaviorTree>
 
   <!-- Description of Node Models (used by Groot) -->
-  <TreeNodesModel>
-    <Action ID="RotateSimple">
-      <input_port name="angle"
-                  type="double"/>
-      <input_port name="radians"
-                  default="false"
-                  type="bool"/>
-      <input_port name="min_angle"
-                  default="0.000000"
-                  type="double"/>
-      <input_port name="action_name"
-                  type="std::string">Action server name</input_port>
-    </Action>
-  </TreeNodesModel>
 
 </root>

toid_bt/behavior_trees/calibration_routines.xml

@@ -3,20 +3,38 @@
   <BehaviorTree ID="wheel_ratio_calibration">
     <Sequence>
       <Delay delay_msec="5000">
-        <ZeroOdom service_name=""/>
+        <RotateSimple angle="0"
+                      max_speed="0.000000"
+                      min_angle="270"
+                      action_name=""/>
       </Delay>
-      <MovePointSimple x="1.0"
+      <Sequence>
-                       y="0"
+        <MovePointSimple x="1.0"
-                       theta="0"
+                         y="0"
-                       action_name=""/>
+                         theta="0"
-      <RotateSimple angle="180"
+                         max_speed="0.10000"
-                    radians="false"
+                         action_name=""/>
-                    min_angle="0.000000"
+        <RotateSimple angle="180"
+                      max_speed="0.500000"
+                      min_angle="10"
+                      action_name=""/>
+        <MovePointSimple x="0.4"
+                         y="0"
+                         theta="180"
+                         max_speed="0.100000"
+                         action_name=""/>
+        <RotateSimple angle="0"
+                      max_speed="0.500000"
+                      min_angle="-10"
+                      action_name=""/>
+      </Sequence>
+      <TranslateX x="-0.5"
+                  max_speed="0.050000"
+                  action_name=""/>
+      <RotateSimple angle="0"
+                    max_speed="0.000000"
+                    min_angle="-270"
                     action_name=""/>
-      <MovePointSimple x="0.4"
-                       y="0"
-                       theta="180"
-                       action_name=""/>
     </Sequence>
   </BehaviorTree>
 
@@ -29,24 +47,32 @@
                   type="double"/>
       <input_port name="theta"
                   type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
     <Action ID="RotateSimple">
       <input_port name="angle"
                   type="double"/>
-      <input_port name="radians"
+      <input_port name="max_speed"
-                  default="false"
+                  default="0.000000"
-                  type="bool"/>
+                  type="double"/>
       <input_port name="min_angle"
                   default="0.000000"
                   type="double"/>
       <input_port name="action_name"
                   type="std::string">Action server name</input_port>
     </Action>
-    <Action ID="ZeroOdom">
+    <Action ID="TranslateX">
-      <input_port name="service_name"
+      <input_port name="x"
-                  type="std::string">Service name</input_port>
+                  type="double"/>
+      <input_port name="max_speed"
+                  default="0.000000"
+                  type="double"/>
+      <input_port name="action_name"
+                  type="std::string">Action server name</input_port>
     </Action>
   </TreeNodesModel>
 

toid_bt/behavior_trees/toid_behaviors.btproj

@@ -11,18 +11,19 @@
             <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="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="RotateSimple">
             <input_port name="angle" type="double"/>
-            <input_port name="radians" default="false" type="bool"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
             <input_port name="min_angle" default="0.000000" type="double"/>
             <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
         <Action ID="RotateTowards">
             <input_port name="x" type="double"/>
             <input_port name="y" type="double"/>
-            <input_port name="radians" default="false" type="bool"/>
+            <input_port name="max_speed" default="0.000000" type="double"/>
             <input_port name="min_angle" default="0.000000" type="double"/>
             <input_port name="action_name" type="std::string">Action server name</input_port>
         </Action>
@@ -32,6 +33,11 @@
             <output_port name="new_width" type="double"/>
             <input_port name="service_name" type="std::string">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="action_name" type="std::string">Action server name</input_port>
+        </Action>
         <Action ID="ZeroOdom">
             <input_port name="service_name" type="std::string">Service name</input_port>
         </Action>

toid_bt/include/toid_bt/plugins/end_calib_action.hpp

@@ -23,7 +23,8 @@ public:
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts({
-      BT::InputPort<double>("width"), BT::InputPort<int>("count", 1, {}),
+      BT::InputPort<double>("width"),
+      BT::InputPort<int>("count", 1, {}),
       BT::OutputPort<double>("new_width"),
       //BT::InputPort<double>("options"),
     });

toid_bt/include/toid_bt/plugins/move_coords_action.hpp

@@ -24,7 +24,10 @@ public:
   static BT::PortsList providedPorts()
   {
     return providedBasicPorts({
-      BT::InputPort<double>("x"), BT::InputPort<double>("y"), BT::InputPort<double>("theta"),
+      BT::InputPort<double>("x"),
+      BT::InputPort<double>("y"),
+      BT::InputPort<double>("theta"),
+      BT::InputPort<double>("max_speed", 0, {}),
       //BT::InputPort<double>("options"),
     });
   }
@@ -34,6 +37,8 @@ public:
     auto x_goal = getInput<double>("x");
     auto y_goal = getInput<double>("y");
     auto theta = getInput<double>("theta");
+    auto max_speed = getInput<double>("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;
   }
 

toid_bt/include/toid_bt/plugins/rotate_action.hpp

@@ -25,7 +25,7 @@ public:
     return providedBasicPorts({
       BT::InputPort<double>("angle", {}), 
       BT::InputPort<double>("min_angle", 0, {}),
-      BT::InputPort<bool>("radians", false, {}),
+      BT::InputPort<double>("max_speed", 0, {}),
       //BT::InputPort<double>("options"),
     });
   }
@@ -34,13 +34,12 @@ public:
   {
     auto angle = getInput<double>("angle");
     auto min_angle = getInput<double>("min_angle");
-    auto radians = getInput<bool>("radians");
+    auto max_speed = getInput<double>("max_speed");
     goal.angle = angle.value();
     goal.min_angle = min_angle.value();
-    if (!radians.value()) {
+    goal.max_speed = max_speed.value();
-      goal.angle = angles::from_degrees(goal.angle);
+    goal.angle = angles::from_degrees(goal.angle);
-      goal.min_angle = angles::from_degrees(goal.min_angle);
+    goal.min_angle = angles::from_degrees(goal.min_angle);
-    }
     return true;
   }
 

toid_bt/include/toid_bt/plugins/rotate_towards_action.hpp

@@ -26,7 +26,7 @@ public:
       BT::InputPort<double>("x", {}), 
       BT::InputPort<double>("y", {}), 
       BT::InputPort<double>("min_angle", 0, {}),
-      BT::InputPort<bool>("radians", false, {}),
+      BT::InputPort<double>("max_speed", 0, {}),
       //BT::InputPort<double>("options"),
     });
   }
@@ -35,18 +35,15 @@ public:
   {
     auto x = getInput<double>("x").value();
     auto y = getInput<double>("y").value();
-    auto min_angle = getInput<double>("min_angle");
+    auto min_angle = getInput<double>("min_angle").value();
-    auto radians = getInput<bool>("radians");
+    auto max_speed = getInput<double>("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();
+    goal.min_angle = angles::from_degrees(min_angle);
-
+    goal.max_speed = max_speed;
-    if (!radians.value()) {
-      goal.min_angle = angles::from_degrees(goal.min_angle);
-    }
 
     return true;
   }

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<toid_msgs::action::SimpleTranslateX>
+{
+public:
+  TranslateXNode(
+    const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
+    PoseFunc pose)
+  : BT::RosActionNode<toid_msgs::action::SimpleTranslateX>(name, conf, params), get_pose(pose)
+  {
+  }
+
+  static BT::PortsList providedPorts()
+  {
+    return providedBasicPorts({
+      BT::InputPort<double>("x"),
+      BT::InputPort<double>("max_speed", 0, {}),
+      //BT::InputPort<double>("options"),
+    });
+  }
+
+  bool setGoal(Goal & goal) override
+  {
+    auto x_goal = getInput<double>("x").value();
+    auto max_speed = getInput<double>("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

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<toid::RotateTowardsNode>(
     "RotateTowards", BT::RosNodeParams(nh, "/rotate"), get_pose);
 
+  factory.registerNodeType<toid::TranslateXNode>(
+    "TranslateX", BT::RosNodeParams(nh, "/translateX"), get_pose);
+
   factory.registerNodeType<toid::EndCalibNode>(
     "SetWidth", BT::RosNodeParams(nh, "/set_width"), get_pose);
 
-  factory.registerNodeType<toid::EmptySrvNode>(
+  factory.registerNodeType<toid::EmptySrvNode>("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
-    "ZeroOdom", BT::RosNodeParams(nh, "/zero"));
 
-  factory.registerNodeType<toid::EmptySrvNode>(
+  factory.registerNodeType<toid::EmptySrvNode>("EndCalib", BT::RosNodeParams(nh, "/end_calib"));
-    "EndCalib", BT::RosNodeParams(nh, "/end_calib"));
 
   std::cout << describeCustomNodes() << std::endl;
-
 }
 
 void TreeExecutor::position(geometry_msgs::msg::PoseStamped & pose)