Robot position control node

mep3_behavior/include/mep3_behavior/robot_position_control.hpp

@@ -0,0 +1,59 @@
+// Copyright 2024 Memristor Robotics
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef MEP3_BEHAVIOR_TREE__ROBOT_POSITION_CONTROL_HPP_
+#define MEP3_BEHAVIOR_TREE__ROBOT_POSITION_CONTROL_HPP_
+
+#include <string>
+#include <vector>
+
+#include "behaviortree_cpp/control_node.h"
+
+namespace mep3_behavior
+{
+    class RobotPositionControl : public BT::ControlNode
+    {
+    public:
+        explicit RobotPositionControl(const std::string &name);
+
+        ~RobotPositionControl() override = default;
+
+        void halt() override;
+
+    private:
+        BT::NodeStatus tick() override;
+    };
+
+    RobotPositionControl::RobotPositionControl(const std::string &name)
+        : ControlNode::ControlNode(name, {})
+    {
+        setRegistrationID("RobotPositionControl");
+        // node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+        // scoreboard_task_pub_ = node_->create_publisher<mep3_msgs::msg::Scoreboard>(
+        // "/scoreboard", rclcpp::SystemDefaultsQoS()
+        // );
+    }
+
+    BT::NodeStatus RobotPositionControl::tick()
+    {
+        setStatus(BT::NodeStatus::RUNNING);
+        return BT::NodeStatus::SUCCESS;
+    }
+
+    void RobotPositionControl::halt()
+    {
+        BT::ControlNode::halt();
+    }
+} // namespace mep3_behavior
+#endif // MEP3_BEHAVIOR_TREE__ROBOT_POSITION_CONTROL_HPP_

mep3_behavior/src/mep3_behavior_tree.cpp

@@ -37,6 +37,7 @@
 #include "mep3_behavior/navigate_to_action.hpp"
 #include "mep3_behavior/scoreboard_task_action.hpp"
 #include "mep3_behavior/task_sequence_control.hpp"
+#include "mep3_behavior/robot_position_control.hpp"
 #include "mep3_behavior/pump_action.hpp"
 #include "mep3_behavior/wait_match_start_action.hpp"
 #include "mep3_behavior/delay_action.hpp"
@@ -150,6 +151,8 @@ int main(int argc, char **argv)
       "AddObstacle");
   factory.registerNodeType<mep3_behavior::RemoveObstacleAction>(
       "RemoveObstacle");
+  factory.registerNodeType<mep3_behavior::RobotPositionControl>(
+      "RobotPosition");
 
   using std::filesystem::directory_iterator;
   for (auto const &entry : directory_iterator(ASSETS_DIRECTORY))