diff --git a/mep3_behavior/include/mep3_behavior/robot_position_control.hpp b/mep3_behavior/include/mep3_behavior/robot_position_control.hpp new file mode 100644 index 00000000..c8bcd30f --- /dev/null +++ b/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 +#include + +#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("node"); + // scoreboard_task_pub_ = node_->create_publisher( + // "/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_ diff --git a/mep3_behavior/src/mep3_behavior_tree.cpp b/mep3_behavior/src/mep3_behavior_tree.cpp index 8db74432..77671456 100644 --- a/mep3_behavior/src/mep3_behavior_tree.cpp +++ b/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( "RemoveObstacle"); + factory.registerNodeType( + "RobotPosition"); using std::filesystem::directory_iterator; for (auto const &entry : directory_iterator(ASSETS_DIRECTORY))