Modified behavior tree nodes to include max_speed

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.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;
   }
 

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 radians = getInput<bool>("radians");
+    auto min_angle = getInput<double>("min_angle").value();
+    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();
-
-    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;
   }

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