Added mg_bt
This commit is contained in:
3
.gitmodules
vendored
Normal file
3
.gitmodules
vendored
Normal file
@ -0,0 +1,3 @@
|
|||||||
|
[submodule "ext/BehaviorTreeRos2"]
|
||||||
|
path = ext/BehaviorTreeRos2
|
||||||
|
url = https://github.com/BehaviorTree/BehaviorTree.ROS2.git
|
||||||
1
ext/BehaviorTreeRos2
Submodule
1
ext/BehaviorTreeRos2
Submodule
Submodule ext/BehaviorTreeRos2 added at 7a6ace6424
67
mg_bt/CMakeLists.txt
Normal file
67
mg_bt/CMakeLists.txt
Normal file
@ -0,0 +1,67 @@
|
|||||||
|
cmake_minimum_required(VERSION 3.12)
|
||||||
|
project(mg_bt)
|
||||||
|
|
||||||
|
set(CMAKE_CXX_STANDARD 17)
|
||||||
|
set(CMAKE_CXX_STANDARD_REQUIRED ON)
|
||||||
|
|
||||||
|
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||||
|
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||||
|
endif()
|
||||||
|
|
||||||
|
set(PACKAGE_DEPS
|
||||||
|
rclcpp
|
||||||
|
behaviortree_cpp
|
||||||
|
behaviortree_ros2
|
||||||
|
btcpp_ros2_interfaces
|
||||||
|
tf2
|
||||||
|
tf2_ros
|
||||||
|
tf2_geometry_msgs
|
||||||
|
mg_msgs
|
||||||
|
std_msgs
|
||||||
|
std_srvs
|
||||||
|
)
|
||||||
|
|
||||||
|
find_package(ament_cmake REQUIRED)
|
||||||
|
|
||||||
|
include(FindPkgConfig)
|
||||||
|
pkg_search_module(LIBGLM REQUIRED glm)
|
||||||
|
|
||||||
|
foreach(PACKAGE ${PACKAGE_DEPS})
|
||||||
|
find_package(${PACKAGE} REQUIRED)
|
||||||
|
endforeach()
|
||||||
|
|
||||||
|
set(SOURCES
|
||||||
|
src/mg_bt.cpp
|
||||||
|
src/mg_tree_executor.cpp
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
add_executable(mg_bt_executor ${SOURCES})
|
||||||
|
|
||||||
|
target_include_directories(
|
||||||
|
mg_bt_executor
|
||||||
|
PRIVATE
|
||||||
|
${LIBGLM_INCLUDE_DIRS}
|
||||||
|
include
|
||||||
|
)
|
||||||
|
|
||||||
|
ament_target_dependencies(mg_bt_executor ${PACKAGE_DEPS})
|
||||||
|
|
||||||
|
install( TARGETS
|
||||||
|
mg_bt_executor
|
||||||
|
DESTINATION lib/${PROJECT_NAME}
|
||||||
|
)
|
||||||
|
|
||||||
|
install(DIRECTORY
|
||||||
|
behavior_trees
|
||||||
|
config
|
||||||
|
launch
|
||||||
|
DESTINATION share/${PROJECT_NAME}/
|
||||||
|
)
|
||||||
|
|
||||||
|
target_compile_features(mg_bt_executor PUBLIC
|
||||||
|
c_std_99
|
||||||
|
cxx_std_17
|
||||||
|
) # Require C99 and C++17
|
||||||
|
|
||||||
|
ament_package()
|
||||||
118
mg_bt/behavior_trees/calib_bt.xml
Normal file
118
mg_bt/behavior_trees/calib_bt.xml
Normal file
@ -0,0 +1,118 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<root BTCPP_format="4">
|
||||||
|
<BehaviorTree ID="calib_bt">
|
||||||
|
<Sequence>
|
||||||
|
<SetBlackboard value="0.31133"
|
||||||
|
output_key="width"/>
|
||||||
|
<Delay delay_msec="20000">
|
||||||
|
<Repeat num_cycles="5">
|
||||||
|
<Sequence>
|
||||||
|
<ZeroNode service_name="/zero"/>
|
||||||
|
<MovePoint action_name="/MovePoint"
|
||||||
|
tolerance="0.04"
|
||||||
|
max_angular="0.1"
|
||||||
|
x_goal="0.7"
|
||||||
|
y_goal="0.0"/>
|
||||||
|
<RotateNode angle="2"
|
||||||
|
action_name="/Rotate"/>
|
||||||
|
<RotateNode angle="-2"
|
||||||
|
action_name="/Rotate"/>
|
||||||
|
<RotateNode angle="0"
|
||||||
|
action_name="/Rotate"/>
|
||||||
|
<Fallback>
|
||||||
|
<Timeout msec="10000">
|
||||||
|
<MovePoint action_name="/MovePoint"
|
||||||
|
max_angular="0.1"
|
||||||
|
x_goal="-0.1"
|
||||||
|
y_goal="0.0"/>
|
||||||
|
</Timeout>
|
||||||
|
<AlwaysSuccess/>
|
||||||
|
</Fallback>
|
||||||
|
<CalibWidth PreviousWidth="{width}"
|
||||||
|
Count="1"
|
||||||
|
NewWidth="{width}"
|
||||||
|
service_name="/set_width"/>
|
||||||
|
</Sequence>
|
||||||
|
</Repeat>
|
||||||
|
</Delay>
|
||||||
|
</Sequence>
|
||||||
|
</BehaviorTree>
|
||||||
|
|
||||||
|
<!-- Description of Node Models (used by Groot) -->
|
||||||
|
<TreeNodesModel>
|
||||||
|
<Action ID="CalibWidth">
|
||||||
|
<input_port name="PreviousWidth"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="Count"
|
||||||
|
default="1"
|
||||||
|
type="int"/>
|
||||||
|
<output_port name="NewWidth"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="service_name"
|
||||||
|
type="std::string">Service name</input_port>
|
||||||
|
</Action>
|
||||||
|
<Action ID="MovePoint">
|
||||||
|
<input_port name="action_name"
|
||||||
|
type="std::string">Action server name</input_port>
|
||||||
|
<input_port name="tolerance"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="obst_mult_a"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="ornt_mult"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="pos_mult"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="t_ornt_mult"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="obst_mult_c"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="obst_mult_b"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="max_vel"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="max_angular"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="IgnoreList"
|
||||||
|
type="std::string"/>
|
||||||
|
<input_port name="max_wheel_speed"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="x_goal"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="y_goal"
|
||||||
|
type="double"/>
|
||||||
|
</Action>
|
||||||
|
<Action ID="RotateNode">
|
||||||
|
<input_port name="angle"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="pos_mult"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="max_wheel_speed"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="IgnoreList"
|
||||||
|
type="std::string"/>
|
||||||
|
<input_port name="max_angular"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="max_vel"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="obst_mult_b"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="obst_mult_c"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="ornt_mult"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="t_ornt_mult"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="obst_mult_a"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="tolerance"
|
||||||
|
type="double"/>
|
||||||
|
<input_port name="action_name"
|
||||||
|
type="std::string">Action server name</input_port>
|
||||||
|
</Action>
|
||||||
|
<Action ID="ZeroNode">
|
||||||
|
<input_port name="service_name"
|
||||||
|
type="std::string">Service name</input_port>
|
||||||
|
</Action>
|
||||||
|
</TreeNodesModel>
|
||||||
|
|
||||||
|
</root>
|
||||||
47
mg_bt/behavior_trees/mg_bt.btproj
Normal file
47
mg_bt/behavior_trees/mg_bt.btproj
Normal file
@ -0,0 +1,47 @@
|
|||||||
|
<?xml version="1.0" encoding="UTF-8"?>
|
||||||
|
<root BTCPP_format="4" project_name="Project">
|
||||||
|
<include path="calib_bt.xml"/>
|
||||||
|
<!-- Description of Node Models (used by Groot) -->
|
||||||
|
<TreeNodesModel>
|
||||||
|
<Action ID="CalibWidth">
|
||||||
|
<input_port name="PreviousWidth" type="double"/>
|
||||||
|
<input_port name="Count" default="1" type="int"/>
|
||||||
|
<output_port name="NewWidth" type="double"/>
|
||||||
|
<input_port name="service_name" type="std::string">Service name</input_port>
|
||||||
|
</Action>
|
||||||
|
<Action ID="MovePoint">
|
||||||
|
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||||
|
<input_port name="tolerance" type="double"/>
|
||||||
|
<input_port name="obst_mult_a" type="double"/>
|
||||||
|
<input_port name="ornt_mult" type="double"/>
|
||||||
|
<input_port name="pos_mult" type="double"/>
|
||||||
|
<input_port name="t_ornt_mult" type="double"/>
|
||||||
|
<input_port name="obst_mult_c" type="double"/>
|
||||||
|
<input_port name="obst_mult_b" type="double"/>
|
||||||
|
<input_port name="max_vel" type="double"/>
|
||||||
|
<input_port name="max_angular" type="double"/>
|
||||||
|
<input_port name="IgnoreList" type="std::string"/>
|
||||||
|
<input_port name="max_wheel_speed" type="double"/>
|
||||||
|
<input_port name="x_goal" type="double"/>
|
||||||
|
<input_port name="y_goal" type="double"/>
|
||||||
|
</Action>
|
||||||
|
<Action ID="RotateNode">
|
||||||
|
<input_port name="angle" type="double"/>
|
||||||
|
<input_port name="pos_mult" type="double"/>
|
||||||
|
<input_port name="max_wheel_speed" type="double"/>
|
||||||
|
<input_port name="IgnoreList" type="std::string"/>
|
||||||
|
<input_port name="max_angular" type="double"/>
|
||||||
|
<input_port name="max_vel" type="double"/>
|
||||||
|
<input_port name="obst_mult_b" type="double"/>
|
||||||
|
<input_port name="obst_mult_c" type="double"/>
|
||||||
|
<input_port name="ornt_mult" type="double"/>
|
||||||
|
<input_port name="t_ornt_mult" type="double"/>
|
||||||
|
<input_port name="obst_mult_a" type="double"/>
|
||||||
|
<input_port name="tolerance" type="double"/>
|
||||||
|
<input_port name="action_name" type="std::string">Action server name</input_port>
|
||||||
|
</Action>
|
||||||
|
<Action ID="ZeroNode">
|
||||||
|
<input_port name="service_name" type="std::string">Service name</input_port>
|
||||||
|
</Action>
|
||||||
|
</TreeNodesModel>
|
||||||
|
</root>
|
||||||
14
mg_bt/config/mg_bt_executor.yaml
Normal file
14
mg_bt/config/mg_bt_executor.yaml
Normal file
@ -0,0 +1,14 @@
|
|||||||
|
bt_action_server:
|
||||||
|
ros__parameters:
|
||||||
|
action_name: "mg_bt_action_server"
|
||||||
|
tick_frequency: 100
|
||||||
|
groot2_port: 8420
|
||||||
|
ros_plugins_timeout: 1000
|
||||||
|
|
||||||
|
plugins:
|
||||||
|
- btcpp_ros2_samples/bt_plugins
|
||||||
|
|
||||||
|
behavior_trees:
|
||||||
|
- mg_bt/behavior_trees
|
||||||
|
|
||||||
|
|
||||||
22
mg_bt/launch/launch.py
Normal file
22
mg_bt/launch/launch.py
Normal file
@ -0,0 +1,22 @@
|
|||||||
|
from launch import LaunchDescription
|
||||||
|
from launch.substitutions import PathJoinSubstitution
|
||||||
|
from launch_ros.actions import Node
|
||||||
|
from launch_ros.substitutions import FindPackageShare
|
||||||
|
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
def generate_launch_description():
|
||||||
|
|
||||||
|
basedir = FindPackageShare("mg_bt")
|
||||||
|
|
||||||
|
bt_exec_config = PathJoinSubstitution([basedir, "config/mg_bt_executor.yaml"])
|
||||||
|
|
||||||
|
return LaunchDescription([
|
||||||
|
Node(
|
||||||
|
package='mg_bt',
|
||||||
|
executable='mg_bt_executor',
|
||||||
|
output="screen",
|
||||||
|
parameters=[bt_exec_config]
|
||||||
|
),
|
||||||
|
])
|
||||||
|
|
||||||
24
mg_bt/package.xml
Normal file
24
mg_bt/package.xml
Normal file
@ -0,0 +1,24 @@
|
|||||||
|
<?xml version="1.0"?>
|
||||||
|
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||||
|
<package format="3">
|
||||||
|
<name>mg_bt</name>
|
||||||
|
<version>0.0.0</version>
|
||||||
|
<description>Behavior for MagRob</description>
|
||||||
|
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
||||||
|
<license>Apache 2.0</license>
|
||||||
|
|
||||||
|
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||||
|
|
||||||
|
<depend>rclcpp</depend>
|
||||||
|
<depend>behaviortree_cpp</depend>
|
||||||
|
<depend>behaviortree_ros2</depend>
|
||||||
|
<depend>btcpp_ros2_interfaces</depend>
|
||||||
|
<depend>mg_msgs</depend>
|
||||||
|
|
||||||
|
<test_depend>ament_lint_auto</test_depend>
|
||||||
|
<test_depend>ament_lint_common</test_depend>
|
||||||
|
|
||||||
|
<export>
|
||||||
|
<build_type>ament_cmake</build_type>
|
||||||
|
</export>
|
||||||
|
</package>
|
||||||
23
mg_bt/src/mg_bt.cpp
Normal file
23
mg_bt/src/mg_bt.cpp
Normal file
@ -0,0 +1,23 @@
|
|||||||
|
#include <iostream>
|
||||||
|
|
||||||
|
#include "mg_tree_executor.hpp"
|
||||||
|
|
||||||
|
int main(const int argc, const char** argv) {
|
||||||
|
rclcpp::init(argc, argv);
|
||||||
|
|
||||||
|
std::cout << "Starting up Behavior Tree Executor" << std::endl;
|
||||||
|
|
||||||
|
rclcpp::NodeOptions options;
|
||||||
|
|
||||||
|
auto bt_exec_server = std::make_shared<mg::MgTreeExecutor>(options);
|
||||||
|
|
||||||
|
rclcpp::executors::MultiThreadedExecutor executor(
|
||||||
|
rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
|
||||||
|
|
||||||
|
executor.add_node(bt_exec_server->node());
|
||||||
|
executor.spin();
|
||||||
|
executor.remove_node(bt_exec_server->node());
|
||||||
|
|
||||||
|
rclcpp::shutdown();
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
59
mg_bt/src/mg_tree_executor.cpp
Normal file
59
mg_bt/src/mg_tree_executor.cpp
Normal file
@ -0,0 +1,59 @@
|
|||||||
|
#include "mg_tree_executor.hpp"
|
||||||
|
|
||||||
|
#include "behaviortree_cpp/xml_parsing.h"
|
||||||
|
#include "tree_nodes/calib.hpp"
|
||||||
|
#include "tree_nodes/move_point.hpp"
|
||||||
|
#include "tree_nodes/rotate.hpp"
|
||||||
|
#include "tree_nodes/zero.hpp"
|
||||||
|
|
||||||
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||||
|
#include "tf2/LinearMath/Transform.h"
|
||||||
|
#include "tf2/convert.h"
|
||||||
|
|
||||||
|
namespace mg {
|
||||||
|
MgTreeExecutor::MgTreeExecutor(const rclcpp::NodeOptions opts) : BT::TreeExecutionServer(opts) {
|
||||||
|
executeRegistration();
|
||||||
|
std::cout << BT::writeTreeNodesModelXML(factory()) << std::endl;
|
||||||
|
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(node()->get_clock());
|
||||||
|
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
|
||||||
|
|
||||||
|
const std::function<std::pair<glm::vec2, double>()> func = std::bind(&MgTreeExecutor::position, this);
|
||||||
|
}
|
||||||
|
|
||||||
|
void MgTreeExecutor::onTreeCreated(BT::Tree& tree) { logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree); }
|
||||||
|
|
||||||
|
void MgTreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) {
|
||||||
|
factory.registerNodeType<mg::MovePointNode>("MovePoint", node());
|
||||||
|
factory.registerNodeType<mg::RotateNode>("RotateNode", node());
|
||||||
|
factory.registerNodeType<mg::ZeroNode>("ZeroNode", node());
|
||||||
|
factory.registerNodeType<mg::CalibWidthNode>("CalibWidth", node(), [this]() { return this->position(); });
|
||||||
|
}
|
||||||
|
|
||||||
|
std::pair<glm::vec2, double> MgTreeExecutor::position() {
|
||||||
|
double x = NAN;
|
||||||
|
double y = NAN;
|
||||||
|
double theta;
|
||||||
|
glm::vec2 pos;
|
||||||
|
|
||||||
|
auto tmsg = tf_buffer_->lookupTransform("odom", "base-link", tf2::TimePointZero);
|
||||||
|
|
||||||
|
tf2::Transform t;
|
||||||
|
tf2::convert(tmsg.transform, t);
|
||||||
|
t.getBasis().getRPY(x, y, theta);
|
||||||
|
auto vec3 = tmsg.transform.translation;
|
||||||
|
|
||||||
|
pos = glm::dvec2(vec3.x, vec3.y);
|
||||||
|
return { pos, theta };
|
||||||
|
}
|
||||||
|
|
||||||
|
std::optional<std::string> MgTreeExecutor::onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) {
|
||||||
|
(void)status;
|
||||||
|
logger_cout_.reset();
|
||||||
|
|
||||||
|
if (was_cancelled) {
|
||||||
|
std::cout << "Behavior tree was cancled" << std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
|
return std::nullopt;
|
||||||
|
}
|
||||||
|
}
|
||||||
25
mg_bt/src/mg_tree_executor.hpp
Normal file
25
mg_bt/src/mg_tree_executor.hpp
Normal file
@ -0,0 +1,25 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "behaviortree_ros2/tree_execution_server.hpp"
|
||||||
|
#include "behaviortree_cpp/loggers/bt_cout_logger.h"
|
||||||
|
#include "tf2_ros/buffer.h"
|
||||||
|
#include "tf2_ros/transform_listener.h"
|
||||||
|
#include "glm/glm.hpp"
|
||||||
|
|
||||||
|
namespace mg {
|
||||||
|
class MgTreeExecutor : public BT::TreeExecutionServer {
|
||||||
|
public:
|
||||||
|
MgTreeExecutor(const rclcpp::NodeOptions opts);
|
||||||
|
void onTreeCreated(BT::Tree& tree) override;
|
||||||
|
void registerNodesIntoFactory(BT::BehaviorTreeFactory& factory) override;
|
||||||
|
|
||||||
|
std::optional<std::string> onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override;
|
||||||
|
|
||||||
|
std::pair<glm::vec2, double> position();
|
||||||
|
|
||||||
|
private:
|
||||||
|
std::shared_ptr<BT::StdCoutLogger> logger_cout_;
|
||||||
|
tf2_ros::Buffer::SharedPtr tf_buffer_;
|
||||||
|
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
|
||||||
|
};
|
||||||
|
}
|
||||||
44
mg_bt/src/tree_nodes/calib.hpp
Normal file
44
mg_bt/src/tree_nodes/calib.hpp
Normal file
@ -0,0 +1,44 @@
|
|||||||
|
#pragma once
|
||||||
|
#include "behaviortree_ros2/bt_service_node.hpp"
|
||||||
|
#include "mg_msgs/srv/send_double.hpp"
|
||||||
|
#include "glm/glm.hpp"
|
||||||
|
|
||||||
|
namespace mg {
|
||||||
|
|
||||||
|
using PosFuncSig = std::function<std::pair<glm::vec2, double>()>;
|
||||||
|
|
||||||
|
class CalibWidthNode : public BT::RosServiceNode<mg_msgs::srv::SendDouble> {
|
||||||
|
public:
|
||||||
|
CalibWidthNode(const std::string& name,
|
||||||
|
const BT::NodeConfig& conf,
|
||||||
|
const BT::RosNodeParams& params,
|
||||||
|
const PosFuncSig pos_query) :
|
||||||
|
BT::RosServiceNode<mg_msgs::srv::SendDouble>(name, conf, params), pos_query_(pos_query) { }
|
||||||
|
|
||||||
|
static BT::PortsList providedPorts() {
|
||||||
|
return providedBasicPorts({ BT::InputPort<double>("PreviousWidth"),
|
||||||
|
BT::InputPort<int>("Count", 1, {}),
|
||||||
|
BT::OutputPort<double>("NewWidth") });
|
||||||
|
}
|
||||||
|
|
||||||
|
bool setRequest(typename Request::SharedPtr& request) override {
|
||||||
|
auto [pos, theta] = pos_query_();
|
||||||
|
double width = getInput<double>("PreviousWidth").value();
|
||||||
|
int count = getInput<int>("Count").value();
|
||||||
|
double new_width = width * (1 + (theta / (2 * M_PI * count)));
|
||||||
|
|
||||||
|
RCLCPP_INFO(logger(), "Setting width to: %lf", new_width);
|
||||||
|
|
||||||
|
request->set__data(count);
|
||||||
|
setOutput("Count", new_width);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
|
||||||
|
return BT::NodeStatus::SUCCESS;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
const PosFuncSig pos_query_;
|
||||||
|
};
|
||||||
|
}
|
||||||
70
mg_bt/src/tree_nodes/move_point.hpp
Normal file
70
mg_bt/src/tree_nodes/move_point.hpp
Normal file
@ -0,0 +1,70 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "behaviortree_ros2/bt_action_node.hpp"
|
||||||
|
#include "mg_msgs/action/move_point.hpp"
|
||||||
|
|
||||||
|
namespace mg {
|
||||||
|
|
||||||
|
class MovePointNode : public BT::RosActionNode<mg_msgs::action::MovePoint> {
|
||||||
|
public:
|
||||||
|
MovePointNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
||||||
|
BT::RosActionNode<mg_msgs::action::MovePoint>(name, conf, params) { }
|
||||||
|
|
||||||
|
static BT::PortsList providedPorts() {
|
||||||
|
return providedBasicPorts({ BT::InputPort<double>("x_goal"),
|
||||||
|
BT::InputPort<double>("y_goal"),
|
||||||
|
BT::InputPort<double>("tolerance"),
|
||||||
|
BT::InputPort<double>("max_wheel_speed"),
|
||||||
|
BT::InputPort<double>("max_angular"),
|
||||||
|
BT::InputPort<double>("max_vel"),
|
||||||
|
BT::InputPort<double>("pos_mult"),
|
||||||
|
BT::InputPort<double>("ornt_mult"),
|
||||||
|
BT::InputPort<double>("t_ornt_mult"),
|
||||||
|
BT::InputPort<double>("obst_mult_a"),
|
||||||
|
BT::InputPort<double>("obst_mult_b"),
|
||||||
|
BT::InputPort<double>("obst_mult_c"),
|
||||||
|
BT::InputPort<std::string>("IgnoreList", "", {}) });
|
||||||
|
}
|
||||||
|
|
||||||
|
bool setGoal(Goal& goal) override {
|
||||||
|
auto x_goal = getInput<double>("x_goal");
|
||||||
|
auto y_goal = getInput<double>("y_goal");
|
||||||
|
auto tolerance = getInput<double>("tolerance");
|
||||||
|
auto max_wheel_speed = getInput<double>("max_wheel_speed");
|
||||||
|
auto max_angular = getInput<double>("max_angular");
|
||||||
|
auto max_vel = getInput<double>("max_vel");
|
||||||
|
auto pos_mult = getInput<double>("pos_mult");
|
||||||
|
auto ornt_mult = getInput<double>("ornt_mult");
|
||||||
|
auto t_ornt_mult = getInput<double>("t_ornt_mult");
|
||||||
|
auto obst_mult_a = getInput<double>("obst_mult_a");
|
||||||
|
auto obst_mult_b = getInput<double>("obst_mult_b");
|
||||||
|
auto obst_mult_c = getInput<double>("obst_mult_c");
|
||||||
|
goal.x = x_goal.value();
|
||||||
|
goal.y = y_goal.value();
|
||||||
|
goal.tolerance = tolerance.value_or(goal.tolerance);
|
||||||
|
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
|
||||||
|
goal.max_angular = max_angular.value_or(goal.max_angular);
|
||||||
|
goal.max_vel = max_vel.value_or(goal.max_vel);
|
||||||
|
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
|
||||||
|
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
|
||||||
|
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
|
||||||
|
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
|
||||||
|
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
|
||||||
|
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
|
||||||
|
|
||||||
|
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
|
||||||
|
return (wr.result->error == ActionType::Result::SUCCESS) ? 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;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
67
mg_bt/src/tree_nodes/rotate.hpp
Normal file
67
mg_bt/src/tree_nodes/rotate.hpp
Normal file
@ -0,0 +1,67 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "behaviortree_ros2/bt_action_node.hpp"
|
||||||
|
#include "mg_msgs/action/rotate.hpp"
|
||||||
|
|
||||||
|
namespace mg {
|
||||||
|
|
||||||
|
class RotateNode : public BT::RosActionNode<mg_msgs::action::Rotate> {
|
||||||
|
public:
|
||||||
|
RotateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
||||||
|
BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
|
||||||
|
|
||||||
|
static BT::PortsList providedPorts() {
|
||||||
|
return providedBasicPorts({ BT::InputPort<double>("angle"),
|
||||||
|
BT::InputPort<double>("tolerance"),
|
||||||
|
BT::InputPort<double>("max_wheel_speed"),
|
||||||
|
BT::InputPort<double>("max_angular"),
|
||||||
|
BT::InputPort<double>("max_vel"),
|
||||||
|
BT::InputPort<double>("pos_mult"),
|
||||||
|
BT::InputPort<double>("ornt_mult"),
|
||||||
|
BT::InputPort<double>("t_ornt_mult"),
|
||||||
|
BT::InputPort<double>("obst_mult_a"),
|
||||||
|
BT::InputPort<double>("obst_mult_b"),
|
||||||
|
BT::InputPort<double>("obst_mult_c"),
|
||||||
|
BT::InputPort<std::string>("IgnoreList", "", {}) });
|
||||||
|
}
|
||||||
|
|
||||||
|
bool setGoal(Goal& goal) override {
|
||||||
|
auto angle = getInput<double>("angle");
|
||||||
|
auto tolerance = getInput<double>("tolerance");
|
||||||
|
auto max_wheel_speed = getInput<double>("max_wheel_speed");
|
||||||
|
auto max_angular = getInput<double>("max_angular");
|
||||||
|
auto max_vel = getInput<double>("max_vel");
|
||||||
|
auto pos_mult = getInput<double>("pos_mult");
|
||||||
|
auto ornt_mult = getInput<double>("ornt_mult");
|
||||||
|
auto t_ornt_mult = getInput<double>("t_ornt_mult");
|
||||||
|
auto obst_mult_a = getInput<double>("obst_mult_a");
|
||||||
|
auto obst_mult_b = getInput<double>("obst_mult_b");
|
||||||
|
auto obst_mult_c = getInput<double>("obst_mult_c");
|
||||||
|
goal.angle = angle.value();
|
||||||
|
goal.tolerance = tolerance.value_or(goal.tolerance);
|
||||||
|
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
|
||||||
|
goal.max_angular = max_angular.value_or(goal.max_angular);
|
||||||
|
goal.max_vel = max_vel.value_or(goal.max_vel);
|
||||||
|
goal.pos_mult = pos_mult.value_or(goal.pos_mult);
|
||||||
|
goal.ornt_mult = ornt_mult.value_or(goal.ornt_mult);
|
||||||
|
goal.t_ornt_mult = t_ornt_mult.value_or(goal.t_ornt_mult);
|
||||||
|
goal.obst_mult_a = obst_mult_a.value_or(goal.obst_mult_a);
|
||||||
|
goal.obst_mult_b = obst_mult_b.value_or(goal.obst_mult_b);
|
||||||
|
goal.obst_mult_c = obst_mult_c.value_or(goal.obst_mult_c);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
|
||||||
|
|
||||||
|
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
|
||||||
|
return (wr.result->error == ActionType::Result::SUCCESS) ? 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;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
20
mg_bt/src/tree_nodes/zero.hpp
Normal file
20
mg_bt/src/tree_nodes/zero.hpp
Normal file
@ -0,0 +1,20 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include "behaviortree_ros2/bt_service_node.hpp"
|
||||||
|
#include "std_srvs/srv/empty.hpp"
|
||||||
|
|
||||||
|
namespace mg {
|
||||||
|
|
||||||
|
class ZeroNode : public BT::RosServiceNode<std_srvs::srv::Empty> {
|
||||||
|
public:
|
||||||
|
ZeroNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
||||||
|
BT::RosServiceNode<std_srvs::srv::Empty>(name, conf, params) { }
|
||||||
|
|
||||||
|
bool setRequest(typename Request::SharedPtr&) override { return true; }
|
||||||
|
|
||||||
|
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr&) override {
|
||||||
|
return BT::NodeStatus::SUCCESS;
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
}
|
||||||
Reference in New Issue
Block a user