From a7b83895571cac7af2f1bfb3791545d268a9584a Mon Sep 17 00:00:00 2001 From: Bastian Hofmann Date: Mon, 12 Sep 2022 13:26:25 +0200 Subject: [PATCH] Attempt to use new plugin format for tree nodes. Crude support for gazebo ignition --- src/btree_nodes/CMakeLists.txt | 20 ++ src/btree_nodes/actorTree.xml | 2 +- src/btree_nodes/plugins.xml | 5 + src/btree_nodes/src/Factory.cpp | 16 ++ src/btree_nodes/src/Factory.h | 18 ++ src/btree_nodes/src/Tree.cpp | 1 + src/gaz_simulation/world/gaz_ign.sdf | 314 +++++++++++++++++++++++ src/ign_actor_plugin/CMakeLists.txt | 15 ++ src/ign_actor_plugin/src/ActorSystem.cpp | 18 ++ src/ign_actor_plugin/src/ActorSystem.h | 26 ++ 10 files changed, 434 insertions(+), 1 deletion(-) create mode 100644 src/btree_nodes/plugins.xml create mode 100644 src/btree_nodes/src/Factory.cpp create mode 100644 src/btree_nodes/src/Factory.h create mode 100644 src/gaz_simulation/world/gaz_ign.sdf create mode 100644 src/ign_actor_plugin/CMakeLists.txt create mode 100644 src/ign_actor_plugin/src/ActorSystem.cpp create mode 100644 src/ign_actor_plugin/src/ActorSystem.h diff --git a/src/btree_nodes/CMakeLists.txt b/src/btree_nodes/CMakeLists.txt index cfe7999..ab869d2 100644 --- a/src/btree_nodes/CMakeLists.txt +++ b/src/btree_nodes/CMakeLists.txt @@ -21,6 +21,7 @@ set(DEPENDENCIES moveit_ros_planning_interface tf2_geometry_msgs moveit_ros_planning + pluginlib ) foreach(dep IN LISTS DEPENDENCIES) @@ -33,6 +34,7 @@ endforeach() set(CPP_FILES src/Tree.cpp + src/Factory.cpp src/Extensions.cpp src/nodes/WeightedRandomNode.cpp src/nodes/AmICalled.cpp @@ -48,14 +50,25 @@ set(CPP_FILES src/nodes/SetRobotVelocity.cpp ) +add_library(tree_plugins_base src/Factory.cpp) add_executable(tree ${CPP_FILES}) +ament_target_dependencies(tree_plugins_base ${DEPENDENCIES}) ament_target_dependencies(tree ${DEPENDENCIES}) +pluginlib_export_plugin_description_file(behaviortree_cpp_v3 plugins.xml) + #add_executable(talker src/publisher_member_function.cpp) #ament_target_dependencies(talker geometry_msgs rclcpp) #add_executable(listener src/subscriber_member_function.cpp) #ament_target_dependencies(listener geometry_msgs rclcpp) +install(TARGETS + tree_plugins_base + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + install(TARGETS tree DESTINATION lib/${PROJECT_NAME}) @@ -71,4 +84,11 @@ if (BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif () +ament_export_libraries( + tree_plugins_base +) +ament_export_targets( + export_${PROJECT_NAME} +) + ament_package() diff --git a/src/btree_nodes/actorTree.xml b/src/btree_nodes/actorTree.xml index b8d33b2..24c129b 100644 --- a/src/btree_nodes/actorTree.xml +++ b/src/btree_nodes/actorTree.xml @@ -9,7 +9,7 @@ - + diff --git a/src/btree_nodes/plugins.xml b/src/btree_nodes/plugins.xml new file mode 100644 index 0000000..6f84710 --- /dev/null +++ b/src/btree_nodes/plugins.xml @@ -0,0 +1,5 @@ + + + Ye. + + \ No newline at end of file diff --git a/src/btree_nodes/src/Factory.cpp b/src/btree_nodes/src/Factory.cpp new file mode 100644 index 0000000..9f3f5a3 --- /dev/null +++ b/src/btree_nodes/src/Factory.cpp @@ -0,0 +1,16 @@ +// +// Created by bastian on 18.08.22. +// + +#include "Factory.h" + + +BT::ActorNodeFactory::ActorNodeFactory() { + auto YES = [](BT::TreeNode &parent_node) -> BT::NodeStatus { + return BT::NodeStatus::SUCCESS; + }; + + registerSimpleCondition("YES!", YES); +} + +PLUGINLIB_EXPORT_CLASS(BT::ActorNodeFactory, BT::BehaviorTreeFactory) \ No newline at end of file diff --git a/src/btree_nodes/src/Factory.h b/src/btree_nodes/src/Factory.h new file mode 100644 index 0000000..9af2d80 --- /dev/null +++ b/src/btree_nodes/src/Factory.h @@ -0,0 +1,18 @@ +// +// Created by bastian on 18.08.22. +// + +#ifndef BUILD_FACTORY_H +#define BUILD_FACTORY_H + +#include +#include + +namespace BT{ + class ActorNodeFactory : public BT::BehaviorTreeFactory { + public: + ActorNodeFactory(); + }; +} + +#endif //BUILD_FACTORY_H diff --git a/src/btree_nodes/src/Tree.cpp b/src/btree_nodes/src/Tree.cpp index 6eb6275..e615bf9 100644 --- a/src/btree_nodes/src/Tree.cpp +++ b/src/btree_nodes/src/Tree.cpp @@ -17,6 +17,7 @@ #include + int main(int argc, char **argv) { rclcpp::init(argc, argv); rclcpp::NodeOptions node_options; diff --git a/src/gaz_simulation/world/gaz_ign.sdf b/src/gaz_simulation/world/gaz_ign.sdf new file mode 100644 index 0000000..8b400e3 --- /dev/null +++ b/src/gaz_simulation/world/gaz_ign.sdf @@ -0,0 +1,314 @@ + + + + + + 0.001 + 1.0 + + + + + ogre2 + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + 1 + + 0 + 3 8 1.2 0 0 0 + + + + 10.1 .1 2.4 + + + + + + + 10.1 .1 2.4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 + 3 -2 1.2 0 0 0 + + + + 10.1 .1 2.4 + + + + + + + 10.1 .1 2.4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 + 8 3 1.2 0 0 0 + + + + .1 9.9 2.4 + + + + + + + .1 9.9 2.4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + 0 + -2 3 1.2 0 0 0 + + + + .1 9.9 2.4 + + + + + + + .1 9.9 2.4 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + 1 + + 1 -1.85 1 0 0 1.5707963267949 + + laser_1 + 10 + + + + 270 + 1 + -2.3561944901923 + 2.3561944901923 + + + 1 + 1 + 0 + 0 + + + + 0.05 + 10.0 + 0.05 + + + 1 + true + + + + + 1 + + -1.85 1 1 0 0 0 + + laser_2 + 10 + + + + 270 + 1 + -2.3561944901923 + 2.3561944901923 + + + 1 + 1 + 0 + 0 + + + + 0.05 + 10.0 + 0.05 + + + 1 + true + + + + + 1 + + + + + 0 0 1 + 100 100 + + + + + 65535 + + + + + 100 + 50 + + + + + + + + 10 + + + 0 + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + 0 + 0 + 0 + + + + + 0 1 1.25 0 0 0 + + + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + 1.0 + + + https://fuel.ignitionrobotics.org/1.0/Mingfei/models/actor/tip/files/meshes/walk.dae + + + + 0 0 -9.8 + 6e-06 2.3e-05 -4.2e-05 + + + 0.001 + 1 + 1000 + + + 0.4 0.4 0.4 1 + 0.7 0.7 0.7 1 + 1 + + + + EARTH_WGS84 + 0 + 0 + 0 + 0 + + + 21 655000000 + 21 863005214 + 1644846646 70909467 + 21655 + + 0 0 0 0 -0 0 + 1 1 1 + + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + 0 0 0 0 -0 0 + + + + 0 0 10 0 -0 0 + + + + + 5 -5 2 0 0.275643 2.35619 + orbit + perspective + + + + diff --git a/src/ign_actor_plugin/CMakeLists.txt b/src/ign_actor_plugin/CMakeLists.txt new file mode 100644 index 0000000..0be313d --- /dev/null +++ b/src/ign_actor_plugin/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.24) +project(ActorPlugin) + +set(IGN_PLUGIN_VER 0) +find_package(ignition-cmake2 REQUIRED) +find_package(ignition-gazebo5 REQUIRED) +ign_find_package(ignition-plugin1 REQUIRED COMPONENTS register) +set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR}) +# Add sources for each plugin to be registered. +add_library(SampleSystem src/ActorSystem.cpp) +set_property(TARGET SampleSystem PROPERTY CXX_STANDARD 17) +target_link_libraries(SampleSystem + ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER} + ignition-gazebo5::ignition-gazebo5 + ) \ No newline at end of file diff --git a/src/ign_actor_plugin/src/ActorSystem.cpp b/src/ign_actor_plugin/src/ActorSystem.cpp new file mode 100644 index 0000000..23f62fe --- /dev/null +++ b/src/ign_actor_plugin/src/ActorSystem.cpp @@ -0,0 +1,18 @@ +// +// Created by bastian on 31.08.22. +// + +#include "ActorSystem.h" + +IGNITION_ADD_PLUGIN( + ActorSystem, + ignition::gazebo::System, + ActorSystem::ISystemPreUpdate) + +ActorSystem::ActorSystem() = default; + +ActorSystem::~ActorSystem() = default; + +void ActorSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info, ignition::gazebo::EntityComponentManager &_ecm) { + +} diff --git a/src/ign_actor_plugin/src/ActorSystem.h b/src/ign_actor_plugin/src/ActorSystem.h new file mode 100644 index 0000000..04f6012 --- /dev/null +++ b/src/ign_actor_plugin/src/ActorSystem.h @@ -0,0 +1,26 @@ +// +// Created by bastian on 31.08.22. +// + +#ifndef BUILD_ACTORSYSTEM_H +#define BUILD_ACTORSYSTEM_H + +#include +#include + +class ActorSystem: + public ignition::gazebo::System, + public ignition::gazebo::ISystemPreUpdate { + +public: + ActorSystem(); + +public: + ~ActorSystem() override; + +public: void PreUpdate(const ignition::gazebo::UpdateInfo &_info, + ignition::gazebo::EntityComponentManager &_ecm) override; +}; + + +#endif //BUILD_ACTORSYSTEM_H